//
//  MathHelper.cpp
//  SGD
//
//  Created by Niketan Pansare on 6/13/13.
//  Copyright (c) 2013 Rice University. All rights reserved.
//

#include "MathHelper.h"

#ifndef DEFAULT_INITIAL_VAL_FOR_SD
#define DEFAULT_INITIAL_VAL_FOR_SD 10
#endif

// UGLY CODE FOR OPTIMIZED REJECTION SAMPLER IN THIS LIBRARY.
// As user of this library, ignore this data structure. This is passed around and used for optimizing rejection samplers.
struct _TempDataStructureForNorm {
  double(*LogPriorFunction)(double par);
  double* mean_x; double* mean_y;
  double* sd_x; double* sd_y;
  double* rho_xy;
  double* x; double* y; unsigned int numValues;
  const char* parameterType;
  
  double* mean_z; double* sd_z; double* z;
  double* rho_yz; double* rho_xz;
  
  double prevReturnValue;
  
  // For 2 dim Normal
  double prevConstantA1;
  double prevConstantA2;
  double prevConstantA3;
  double prevConstantA4;
  double prevConstantA5;
  double prevConstantA6;
  
  // For 3 dim Normal
  double prevConstant1;
  double prevConstant2;
  double prevParamValue;
  bool prevValueInitialized;
  
  // Remember to deallocate them using delete
  double* prevConstant3i;
  double* prevDeltaDashi;
  double* prevDeltaDoubleDashi;
  
  double* prevConstant4Dashi; // = xi - prevConstant4i
  double* prevConstant5i;
  double* prevConstant6i;
  double* prevConstant7i;
  double* prevConstant8i;
};
struct _TempDataStructureForNorm* SD_temp1;

int compare(const void* a, const void* b);
void FindMultiModalMaximaInRange(double(*myFunction)(const std::vector<double>& parameters), double initialValue, double left, double right, double* maxima, double* argMax, enum TypeOfBoundsForOptimization howBounded);

double d3norm(double x, double y, double z, double mean_x, double mean_y, double mean_z, double sd_x, double sd_y, double sd_z, double rho_xy, double rho_xz, double rho_yz, bool returnLog = false);

double GetSigmaBar(double sdx, double rhoxy, double rhoxz, double rhoyz) {
  return pow(sdx, 2) * (1 - (pow(rhoxy, 2) + pow(rhoxz, 2) + pow(rhoyz, 2) - 2*rhoxy*rhoxz*rhoyz)) / (1 - pow(rhoyz, 2));
  // return pow(sdx, 2) * (1 - pow(rhoxy, 2) + pow(rhoxz, 2));
}

double GetDelta(double sdx, double sdy, double sdz, double rhoxy, double rhoxz, double rhoyz, double y, double z, double muy, double muz) {
  return  sdx*(rhoxy - rhoyz*rhoxz)*(y - muy) / (sdy*(1 - pow(rhoyz, 2))) +
  sdx*(rhoxz - rhoyz*rhoxy)*(z - muz) / (sdz*(1 - pow(rhoyz, 2)));
}

int compare(const void* a, const void* b) {
  if(*(double*)a  < *(double*)b) {
    return -1;
  }
  else if(*(double*)a  == *(double*)b) {
    return 0;
  }
  else {
    return 1;
  }
  // Works only for integers: return ( *(double*)a - *(double*)b );
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#ifdef USE_GSL_OPTIMIZER
// UGLY CODE FOR GSL OPTIMIZER IN THIS LIBRARY.
struct _TempDataStructureForGSL {
  std::vector<double>* ptrParams;
  double(*myFunction)(const std::vector<double>& par);
  unsigned int paramIndex;
  bool doMinimization;
};


double myTransformedFunForGSL(double x, void* my_params) {
  struct _TempDataStructureForGSL* ptrTmpDS = (struct _TempDataStructureForGSL*)my_params;
  
  // Set the passed value
  (*(ptrTmpDS -> ptrParams))[(ptrTmpDS -> paramIndex)] = x;
  
  if(ptrTmpDS -> doMinimization) {
    return (*(ptrTmpDS -> myFunction))(*(ptrTmpDS -> ptrParams));
  }
  else {
    return -(*(ptrTmpDS -> myFunction))(*(ptrTmpDS -> ptrParams));
  }
}

bool OneDimensionalOptimizer :: gsl_doesIntervalContainMaxima() {
  double a = l, b = r;
  double temp = ((b-a)/10);
  double delta = temp < 10 ? temp : 10; // To avoid bad delta if b is too large
  double m = a + delta; /// m is the guess if location of maxima, i.e its argMax
  gsl_min_fminimizer* s;
  
  gsl_function F;
  F.function = &myTransformedFunForGSL;
  struct _TempDataStructureForGSL tempDS;
  tempDS.ptrParams = &initialValues;
  tempDS.myFunction = f;
  tempDS.paramIndex = parameterIndex;
  tempDS.doMinimization = doMinimization;
  F.params = &tempDS;
  
  const gsl_min_fminimizer_type* T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc(T);
  gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
  int errCode = gsl_min_fminimizer_set(s, &F, m, a, b);
  gsl_set_error_handler(old_handler);
  
  if(errCode == GSL_FAILURE) {
    return false;
  }
  
  if(errCode == GSL_EINVAL) {
    // Bug#307744 in documentation
    // http://www.mail-archive.com/debian-bugs-dist@lists.debian.org/msg44552.html
    return false;
  }
  return true;
}

bool OneDimensionalOptimizer :: gsl_oneDimensionalMaximization(double* value, double* argVal) {
  int status, max_iter = 1000;
  int iter = 0;
  double a = l, b = r;
  double temp = ((b-a)/10);
  double delta = temp < 10 ? temp : 10; // To avoid bad delta if b is too large
  double m = a + delta; /// m is the guess if location of maxima, i.e its argMax
  gsl_min_fminimizer* s;
  gsl_function F;
  
  F.function = &myTransformedFunForGSL;
  struct _TempDataStructureForGSL tempDS;
  tempDS.ptrParams = &initialValues;
  tempDS.myFunction = f;
  tempDS.paramIndex = parameterIndex;
  tempDS.doMinimization = doMinimization;
  F.params = &tempDS;
  const gsl_min_fminimizer_type* T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc(T);
  gsl_min_fminimizer_set(s, &F, m, a, b);
  
  do {
    iter++;
    status = gsl_min_fminimizer_iterate(s);
    m = gsl_min_fminimizer_x_minimum(s);
    a = gsl_min_fminimizer_x_lower(s);
    b = gsl_min_fminimizer_x_upper(s);
    status = gsl_min_test_interval(a, b, err, 0);
  }
  while(status == GSL_CONTINUE && iter < max_iter);
  gsl_min_fminimizer_free(s);
  *argVal = (a + b)/2;
  *value = myTransformedFunForGSL(*argVal, &tempDS);
  if(status != GSL_SUCCESS) {
    std::stringstream ss;
    ss << "The minima could not be found successfully after " << iter << " iterations. (Maximum set iteration: " << max_iter << ").";
    HANDLE_GSL_ERROR(ss.str())
    return false;
  }
  return true;
}
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


bool OneDimensionalOptimizer :: doOptimization(double* value, double* argVal) {
#if !defined(USE_MINUIT_OPTIMIZER) && !defined(USE_GSL_OPTIMIZER)
  HANDLE_GSL_ERROR("Error: You have to define either (or both) of following: USE_MINUIT_OPTIMIZER and USE_GSL_OPTIMIZER");
  return false;
#endif
  
#ifdef USE_MINUIT_OPTIMIZER
  ROOT::Minuit2::MnUserParameters upar;
  for(int i = 0; i < initialValues.size(); i++) {
    if(i == parameterIndex) {
      upar.Add("arg", initialValues[i], err);
      if(howBounded == HARD_BOTH_SIDES) {
        upar.SetLimits("arg", l, r);
      }
      else if(howBounded == HARD_UPPER) {
        upar.SetUpperLimit("arg", r);
      }
      else if(howBounded == HARD_LOWER) {
        upar.SetLowerLimit("arg", l);
      }
    }
    else {
      std::stringstream ss;
      ss << "p" << i;
      upar.Add(ss.str(), initialValues[i], err);
      // Also fix all other parameters
      upar.Fix(ss.str());
    }
  }
  
  
#ifdef PRECISE_RATHER_THAN_FAST_OPTIMIZATION
  ROOT::Minuit2::MnMinimize migrad(*this, upar, 2);
#else
  ROOT::Minuit2::MnMinimize migrad(*this, upar);
#endif
  ROOT::Minuit2::FunctionMinimum min = migrad();
  
  if(min.IsValid()) {
    *value = min.Fval();
    *argVal = min.UserParameters().Value("arg");
    return true;
  }
  else {
    upar.SetLimits("arg", l, r);
    ROOT::Minuit2::MnMinimize migrad1(*this, upar); // Fast Optimization rather precise since soft bounds
    min = migrad1();
    if(min.IsValid()) {
      *value = min.Fval();
      *argVal = min.UserParameters().Value("arg");
      return true;
    }
    else {
      ROOT::Minuit2::MnScan miscan1(*this, upar, 2); // Fast Optimization rather precise since soft bounds
      min = miscan1();
      if(min.IsValid()) {
        *value = min.Fval();
        *argVal = min.UserParameters().Value("arg");
        return true;
      }
    }
  }
#endif
  
  std::cout << "\nWarning: Minuit couldnot find an optimized value.";
  
#ifdef USE_GSL_OPTIMIZER
  if(gsl_doesIntervalContainMaxima()) {
    return gsl_oneDimensionalMaximization(value, argVal);
  }
#endif
  
#ifdef USE_LAST_ATTEMPT_OPTIMIZER
  HANDLE_GSL_ERROR("Warning: Last attempt optimizer is not yet implemented")
#endif
  
  HANDLE_GSL_ERROR("None of the optimization method is successful")
  return false;
}


// Bounded optimizer:
// howBounded: 0 = [a b]; -1 = (-infinity, b]; 1 = [a, infinity)
OneDimensionalOptimizer :: OneDimensionalOptimizer(double(*myFunction)(const std::vector<double>& parameters), std::vector<double> initialValues1, double tolerance, double lower, double upper, enum TypeOfBoundsForOptimization howBounded1, unsigned int indexOfParameterToBeOptimized) {
  f = myFunction;
  howBounded = howBounded1;
  l = lower;
  r = upper;
  err = tolerance;
  parameterIndex = indexOfParameterToBeOptimized;
  initialValues = initialValues1;
}

// Returns false if unable to find maxima
bool OneDimensionalOptimizer :: FindMaxima(double* maxima, double* argmax) {
  doMinimization = false;
  bool retVal = doOptimization(maxima, argmax);
  *maxima *= -1;
  return retVal;
}

bool OneDimensionalOptimizer :: FindMinima(double* minima, double* argmin) {
  doMinimization = true;
  bool retVal = doOptimization(minima, argmin);
  return retVal;
}


double OneDimensionalOptimizer :: operator()(const std::vector<double>& par) const {
  if(doMinimization) {
    return (*f)(par);
  }
  else {
    return -(*f)(par);
  }
}

double OneDimensionalOptimizer :: Up() const {
  return 1;
}


StatisticsHelper :: StatisticsHelper() {
  global_random_number_generator = gsl_rng_alloc(gsl_rng_mt19937);
}

StatisticsHelper :: ~StatisticsHelper() {
  gsl_rng_free(global_random_number_generator);
}

int StatisticsHelper :: rdunif(int min, int max) {
  return (int)floor(runif(min, max));
}

double StatisticsHelper :: ddunif(int x, int min, int max, bool returnLog) {
  if(returnLog) {
    if(x >= min && x < max) {
      // Since LOG(1) = 0
      return -LOG(max - min);
    }
    else {
      std::stringstream ss;
      ss << "In ddunif, return probability is zero and hence log0 is undefined";
      HANDLE_GSL_ERROR(ss.str())
    }
    
  }
  else {
    if(x >= min && x < max) {
      return 1 / (max - min);
    }
    else {
      return 0;
    }
  }
}


double StatisticsHelper :: runif(double min, double max) {
  return (gsl_rng_uniform(global_random_number_generator) * (max-min) + min);
}

void StatisticsHelper :: rdirichlet(int sizeOfAlpha, double* alpha, double* retVal) {
  if(alpha == NULL || retVal == NULL) {
    std::stringstream ss;
    ss << "In rdirichlet, alpha or retVal cannot be NULL";
    HANDLE_GSL_ERROR(ss.str())
  }
  gsl_ran_dirichlet(global_random_number_generator, sizeOfAlpha, alpha, retVal);
}

double StatisticsHelper :: ddirichlet(int sizeOfAlpha, double* alpha, double* x, bool returnLog) {
  if(alpha == NULL || x == NULL) {
    std::stringstream ss;
    ss << "In ddirichlet, alpha or retVal cannot be NULL";
    HANDLE_GSL_ERROR(ss.str())
  }
  //std::cout << "*(";
  double retVal = 0;
  if(returnLog) {
    retVal = gsl_ran_dirichlet_lnpdf(sizeOfAlpha, alpha, x);
  }
  else {
    retVal = gsl_ran_dirichlet_pdf(sizeOfAlpha, alpha, x);
  }
  if(gsl_finite(retVal) == 0) {
    std::stringstream ss;
    ss << "In ddirichlet, the probability is not finite for parameters -> ddirichlet(" << sizeOfAlpha << ", alpha = [";
    for(int i = 0; i < sizeOfAlpha; i++) {
      ss << alpha[i] << ", ";
    }
    ss << "],  x = [";
    for(int i = 0; i < sizeOfAlpha; i++) {
      ss << x[i] << ", ";
    }
    ss << "], " << returnLog << ")";
    HANDLE_GSL_ERROR(ss.str())
  }
  
  //std::cout << ")*";
  return retVal;
}

void StatisticsHelper :: NormalizeInLogScale(int sizeOfProb, double* logProb) {
  // normalizeLog[i]  = LOG(prob[i] / sum(prob[i]))
  //                  = LOG(prob[i]) - LOG(sum(prob[i])))
  assert(sizeOfProb >= 2);
  double logSumProb = log_add(logProb[0], logProb[1]);
  for(int i = 2; i < sizeOfProb; i++) {
    logSumProb = log_add(logSumProb, logProb[i]);
  }
  
  for(int i = 0; i < sizeOfProb; i++) {
    logProb[i] = logProb[i] - logSumProb;
  }
}

// Returns LOG(a' + b') where a = LOG(a') and b = LOG(b')
double  StatisticsHelper :: log_add(double a, double b) {
  // Note, a > b
  double min = a < b ? a : b;
  double max = a > b ? a : b;
  
  double temp = 1 + exp(min - max);
  if(temp == 0) {
    HANDLE_GSL_ERROR("Problem while adding logs")
  }
  return max + LOG(temp);
}

void StatisticsHelper :: rmultinom(int size, int sizeOfProb, double* prob, unsigned int* retVal, bool isProbInLogSpace) {
  if(prob == NULL || retVal == NULL) {
    std::stringstream ss;
    ss << "In rmultinom, prob or retVal cannot be NULL";
    HANDLE_GSL_ERROR(ss.str())
  }
  if(!isProbInLogSpace) {
    gsl_ran_multinomial(global_random_number_generator, sizeOfProb, size, prob, retVal);
  }
  else {
    double* newProb = new double[sizeOfProb];
    for(int i = 0; i < sizeOfProb; i++) {
      newProb[i] = prob[i];
    }
    unsigned t0 = clock();
    NormalizeInLogScale(sizeOfProb, newProb);
    if((clock() - t0) / (double)CLOCKS_PER_SEC > 0.1) {
      std::cout << "Time to for NormalizeInLogScale:" <<  (clock() - t0) / (double)CLOCKS_PER_SEC  << "\n";
    }
    
    for(int i = 0; i < sizeOfProb; i++) {
      newProb[i] = exp(newProb[i]);
      //std::cout << " " << newProb[i];
      if(newProb[i] < 0) {
        std::stringstream ss;
        ss << "In rmultinom, oldProb[" << i << "]=" << prob[i] << ", but the normalize (and real-spaced) probability is " << newProb[i] << "";
        HANDLE_GSL_ERROR(ss.str())
      }
    }
    t0 = clock();
    gsl_ran_multinomial(global_random_number_generator, sizeOfProb, size, newProb, retVal);
    if((clock() - t0) / (double)CLOCKS_PER_SEC > 0.1) {
      std::cout << "Time to for gsl_ran_multinomial:" <<  (clock() - t0) / (double)CLOCKS_PER_SEC  << "\n";
    }
    std::cout.flush();
    
    delete [] newProb;
  }
}

double StatisticsHelper :: dmultinom(int sizeOfProb, double* prob, unsigned int* x, bool isProbInLogSpace, bool returnLog) {
  if(prob == NULL || x == NULL) {
    std::stringstream ss;
    ss << "In dmultinom, prob or x cannot be NULL";
    HANDLE_GSL_ERROR(ss.str())
  }
  if(!isProbInLogSpace) {
    if(returnLog) {
      return gsl_ran_multinomial_lnpdf(sizeOfProb, prob, x);
    }
    else {
      return gsl_ran_multinomial_pdf(sizeOfProb, prob, x);
    }
  }
  else {
    double* newProb = new double[sizeOfProb];
    for(int i = 0; i < sizeOfProb; i++) {
      newProb[i] = prob[i];
    }
    NormalizeInLogScale(sizeOfProb, newProb);
    for(int i = 0; i < sizeOfProb; i++) {
      newProb[i] = exp(newProb[i]);
      if(newProb[i] <= 0) {
        std::stringstream ss;
        ss << "In dmultinom, oldProb[" << i << "]=" << prob[i] << ", but the normalize (and real-spaced) probability is " << newProb[i] << "";
        HANDLE_GSL_ERROR(ss.str())
      }
    }
    double retVal = dmultinom(sizeOfProb, newProb, x, false, returnLog);
    delete [] newProb;
  }
}

int StatisticsHelper :: rpois(double lambda) {
  return gsl_ran_poisson(global_random_number_generator, lambda);
}

double StatisticsHelper :: dpois(int x, double lambda, bool returnLog) {
  if(returnLog) {
    return x*LOG(lambda) - lambda - lnfactorial(x);
  }
  else {
    return gsl_ran_poisson_pdf(x, lambda);
  }
}

double StatisticsHelper :: lnfactorial(int val) {
  double retVal = 0;
  if(val == 1) {
    return 0;
  }
  else {
    for(int i = 2;  i <= val; i++) {
      retVal += LOG(i);
    }
    
    return retVal;
  }
  
}
double StatisticsHelper :: dunif(double x, double min, double max, bool returnLog) {
  if(x < min || x > max) {
    if(returnLog) {
      // since LOG(0) not defined
      return GSL_NAN;
    }
    else {
      return 0;
    }
  }
  else {
    if(returnLog) {
      return -LOG(max-min);
    }
    else {
      return 1/(max-min);
    }
  }
}

double StatisticsHelper :: rnorm(double mean, double sd) {
  double retVal = gsl_ran_gaussian(global_random_number_generator, sd) + mean;
  return retVal;
}

double StatisticsHelper :: dnorm(double x, double mean, double sd, bool returnLog) {
  assert(sd > 0); // "In logdnorm, sd cannot be negative or zero";
  
  if(returnLog) {
    double t1 = (x - mean) * (x - mean) / (2 * sd * sd);
    // LOG(2*pi) / 2 = 0.9189385
    double t2 = LOG(sd);
    double retVal = - (t1 + t2 + 0.9189385);
    
    assert(IS_FINITE(retVal)); // Return value is infinite or NAN
    
    return retVal;
  }
  else {
    return gsl_ran_gaussian_pdf((x-mean), sd);
  }
}

double StatisticsHelper :: rtnorm(double mean, double sd, double lower) {
  if(lower > mean) {
    // Return upper tail since only works if (lower-mean) is positive
    return gsl_ran_gaussian_tail(global_random_number_generator, lower - mean, sd) + mean;
  }
  else {
    // Bad sampler: TODO Later
    double retVal;
    while(1) {
      retVal = rnorm(mean, sd);
      if(retVal > lower) {
        return retVal;
      }
    }
  }
  
}

double StatisticsHelper :: dtnorm(double x, double mean, double sd, double lower, bool returnLog)  {
  if(returnLog) {
    HANDLE_GSL_ERROR("In dtnorm, log functionality not implemented yet")
    return GSL_NAN;
  }
  else {
    return gsl_ran_gaussian_tail_pdf((x-mean), lower, sd);
  }
}

void StatisticsHelper :: rmnorm(double* retVal, double* means, double* sds, double* rhos, int m) {
  if(retVal == NULL || means == NULL || sds == NULL || rhos == NULL) {
    HANDLE_GSL_ERROR("In function rmnorm, The user is expected to allocate/deallocate the memory for retVal, means, sds, rhos in calling function (i.e they cannot be NULL).")
  }
  
  if(m == 2) {
    // Bivariate normal
    gsl_ran_bivariate_gaussian(global_random_number_generator, sds[0], sds[1], rhos[0], &retVal[0], &retVal[1]);
    retVal[0] += means[0];
    retVal[1] += means[1];
  }
  else if(m == 3) {
    HANDLE_GSL_ERROR("In function rmnorm, Will support it soon ... ")
  }
  else {
    HANDLE_GSL_ERROR("In function rmnorm, only m = 2 or 3 supported as of now")
  }
}

double StatisticsHelper :: cond_rmnorm(double* vals, double* means, double* sds, double* rhos, int m, const char* retVarStr) {
  if(vals == NULL || means == NULL || sds == NULL || rhos == NULL) {
    HANDLE_GSL_ERROR("In function cond_rmnorm, The user is expected to allocate/deallocate the memory for vals, means, sds, rhos in calling function (i.e they cannot be NULL).")
  }
  
  if(m == 2) {
    // Bivariate normal
    int retVarIndex; int condVarIndex;
    if(strcmp(retVarStr, "x") == 0) {
      retVarIndex = 0; condVarIndex = 1;
    }
    else if(strcmp(retVarStr, "y") == 0) {
      retVarIndex = 1; condVarIndex = 0;
    }
    else {
      HANDLE_GSL_ERROR("In function cond_rmnorm, Invalid retValStr (only x, y supported)")
    }
    double newMean = means[retVarIndex] + (sds[retVarIndex] / sds[condVarIndex]) * rhos[0] * (vals[condVarIndex] - means[condVarIndex]);
    double newSd = sqrt(1 - pow(rhos[0],2)) * sds[retVarIndex];
    return rnorm(newMean, newSd);
  }
  else if(m == 3) {
    // Trivariate normal
    int retVarIndex; int condVarIndex1; int condVarIndex2;
    double rho1; double rho2; double rho3;
    if(strcmp(retVarStr, "x") == 0) {
      retVarIndex = 0; condVarIndex1 = 1; condVarIndex2 = 2;
      rho1 = rhos[0]; rho2 = rhos[1]; rho3 = rhos[2];
    }
    else if(strcmp(retVarStr, "y") == 0) {
      retVarIndex = 1; condVarIndex1 = 0; condVarIndex2 = 2;
    }
    else if(strcmp(retVarStr, "z") == 0) {
      retVarIndex = 2; condVarIndex1 = 0; condVarIndex2 = 1;
    }
    else {
      HANDLE_GSL_ERROR("In function cond_rmnorm, Invalid retValStr (only x, y supported)")
    }
    double muBar = GetMuBar(means[retVarIndex], means[condVarIndex1], means[condVarIndex2], sds[retVarIndex], sds[condVarIndex1], sds[condVarIndex2], rho1, rho2, rho3, vals[condVarIndex1], vals[condVarIndex2]);
    double sigmaBar = sqrt(GetSigmaBar(sds[retVarIndex], rho1, rho2, rho3));
    return rnorm(muBar, sigmaBar);
  }
  else {
    HANDLE_GSL_ERROR("In function cond_rmnorm, only m = 2 or 3 supported as of now")
  }
}

// int rmvnorm(const gsl_rng *r, const int n, const gsl_vector *mean, const gsl_matrix *var, gsl_vector *result){
void StatisticsHelper :: rmvnorm(double* retVal, double* mu, double* sigma, int m) {
  MatrixHelper mHelper;
  VectorHelper vHelper;
  
  gsl_vector* mean =  gsl_vector_alloc(m);
  vHelper.convert_rowWise_to_gslVector(mu, mean, m);
  
  gsl_matrix * var = gsl_matrix_calloc(m, m);
  mHelper.convert_rowWise_to_gslMatrix(sigma, var, m, m);
  
  gsl_vector* result = gsl_vector_alloc(m);
  
  // -----------------------------------------------------
  // See http://www.mail-archive.com/help-gsl@gnu.org/msg00631.html
  int k;
  gsl_matrix *work = gsl_matrix_alloc(m,m);
  
  gsl_matrix_memcpy(work,var);
  gsl_linalg_cholesky_decomp(work);
  
  for(k=0; k<m; k++) {
    gsl_vector_set( result, k, gsl_ran_ugaussian(global_random_number_generator) );
  }
  
  gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, work, result);
  gsl_vector_add(result,mean);
  
  gsl_matrix_free(work);
  // -----------------------------------------------------
  
  vHelper.convert_gslVector_to_rowWise(result, retVal, m);
  
  gsl_matrix_free(var);
  gsl_vector_free(mean);
  gsl_vector_free(result);
  
}

//double dmvnorm(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var){
double StatisticsHelper :: dmvnorm(double* val, double* mu, double* sigma, int n, bool returnLog) {
  assert(returnLog == false); // log(dmvnorm) not implemented yet for general m
  
  MatrixHelper mHelper;
  VectorHelper vHelper;
  
  gsl_vector* x =  gsl_vector_alloc(n);
  vHelper.convert_rowWise_to_gslVector(val, x, n);
  
  gsl_vector* mean =  gsl_vector_alloc(n);
  vHelper.convert_rowWise_to_gslVector(mu, mean, n);
  
  gsl_matrix * var = gsl_matrix_calloc(n, n);
  mHelper.convert_rowWise_to_gslMatrix(sigma, var, n, n);
  
  // -----------------------------------------------------
  // See http://www.mail-archive.com/help-gsl@gnu.org/msg00631.html
  int s;
  double ax,ay;
  gsl_vector *ym, *xm;
  gsl_matrix *work = gsl_matrix_alloc(n,n),
  *winv = gsl_matrix_alloc(n,n);
  gsl_permutation *p = gsl_permutation_alloc(n);
  
  gsl_matrix_memcpy( work, var );
  gsl_linalg_LU_decomp( work, p, &s );
  gsl_linalg_LU_invert( work, p, winv );
  ax = gsl_linalg_LU_det( work, s );
  gsl_matrix_free( work );
  gsl_permutation_free( p );
  
  xm = gsl_vector_alloc(n);
  gsl_vector_memcpy( xm, x);
  gsl_vector_sub( xm, mean );
  ym = gsl_vector_alloc(n);
  gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
  gsl_matrix_free( winv );
  gsl_blas_ddot( xm, ym, &ay);
  gsl_vector_free(xm);
  gsl_vector_free(ym);
  ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax );
  // -----------------------------------------------------
  
  gsl_matrix_free(var);
  gsl_vector_free(mean);
  gsl_vector_free(x);
  
  return ay;
}

double StatisticsHelper :: cond_rtmnorm(double* vals, double* means, double* sds, double* rhos, int m, const char* retVarStr, double lower) {
  if(vals == NULL || means == NULL || sds == NULL || rhos == NULL) {
    HANDLE_GSL_ERROR("In function cond_rtmnorm, The user is expected to allocate/deallocate the memory for vals, means, sds, rhos in calling function (i.e they cannot be NULL).")
  }
  
  if(m == 2) {
    // Bivariate normal
    int retVarIndex; int condVarIndex;
    if(strcmp(retVarStr, "x") == 0) {
      retVarIndex = 0; condVarIndex = 1;
    }
    else if(strcmp(retVarStr, "y") == 0) {
      retVarIndex = 1; condVarIndex = 0;
    }
    else {
      HANDLE_GSL_ERROR("In function cond_rtmnorm, Invalid retValStr (only x, y supported)")
    }
    double newMean = means[retVarIndex] + (sds[retVarIndex] / sds[condVarIndex]) * rhos[0] * (vals[condVarIndex] - means[condVarIndex]);
    double newSd = sqrt(1 - pow(rhos[0],2)) * sds[retVarIndex];
    return rtnorm(newMean, newSd, lower);
  }
  else if(m == 3) {
    // Trivariate normal
    int retVarIndex; int condVarIndex1; int condVarIndex2;
    double rho1; double rho2; double rho3;
    if(strcmp(retVarStr, "x") == 0) {
      retVarIndex = 0; condVarIndex1 = 1; condVarIndex2 = 2;
      rho1 = rhos[0]; rho2 = rhos[1]; rho3 = rhos[2];
    }
    else if(strcmp(retVarStr, "y") == 0) {
      retVarIndex = 1; condVarIndex1 = 0; condVarIndex2 = 2;
    }
    else if(strcmp(retVarStr, "z") == 0) {
      retVarIndex = 2; condVarIndex1 = 0; condVarIndex2 = 1;
    }
    else {
      HANDLE_GSL_ERROR("In function cond_rtmnorm, Invalid retValStr (only x, y supported)")
    }
    double muBar = GetMuBar(means[retVarIndex], means[condVarIndex1], means[condVarIndex2], sds[retVarIndex], sds[condVarIndex1], sds[condVarIndex2], rho1, rho2, rho3, vals[condVarIndex1], vals[condVarIndex2]);
    double sigmaBar = sqrt(GetSigmaBar(sds[retVarIndex], rho1, rho2, rho3));
    return rtnorm(muBar, sigmaBar, lower);
  }
  else {
    HANDLE_GSL_ERROR("In function cond_rtmnorm, only m = 2 or 3 supported as of now")
  }
}

double StatisticsHelper :: dmnorm(double* vals, double* means, double* sds, double* rhos, int m, bool returnLog) {
  if(vals == NULL || means == NULL || sds == NULL || rhos == NULL) {
    HANDLE_GSL_ERROR("In function rmnorm, The user is expected to allocate/deallocate the memory for vals, means, sds, rhos in calling function (i.e they cannot be NULL).")
  }
  
  if(m == 2) {
    // Bivariate normal
    if(returnLog) {
      //ln 2pi = 1.83784
      double t1 = (vals[0] - means[0]) / sds[0];
      double t2 = (vals[1] - means[1]) / sds[1];
      double t3 = 1 - pow(rhos[0], 2);
      double t4 = -(pow(t1, 2) + pow(t2, 2) - 2*rhos[0]*t1*t2)/(2*t3);
      return t4 - (1.83784 + LOG(sds[0]) + LOG(sds[1]) + LOG(t3)/2);
    }
    else {
      return gsl_ran_bivariate_gaussian_pdf((vals[0] - means[0]), (vals[1] - means[1]), sds[0], sds[1], rhos[0]);
    }
  }
  else if(m == 3) {
    // Trivariate normal
    return d3norm(vals[0], vals[1], vals[2], means[0], means[1], means[2], sds[0], sds[1], sds[2], rhos[0], rhos[1], rhos[2], returnLog);
  }
  else {
    HANDLE_GSL_ERROR("In function dmnorm, only m = 2 or 3 supported as of now")
  }
  
}

double d3norm(double x, double y, double z, double mean_x, double mean_y, double mean_z, double sd_x, double sd_y, double sd_z, double rho_xy, double rho_xz, double rho_yz, bool returnLog) {
  
  StatisticsHelper sdist;
  
  if(sd_x <= 0 || sd_y <= 0 || sd_z <= 0) {
    std::stringstream ss;
    ss << "In d3norm, sd cannot be less than or equal to 0 sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z;
    HANDLE_GSL_ERROR(ss.str())
  }
  
  double t1 = (sd_x * (rho_xy - rho_yz*rho_xz) * (y - mean_y)) / (sd_y * (1 - pow(rho_yz,2)));
  double t2 = - (sd_x * (rho_xz - rho_yz*rho_xy) * (z - mean_z)) / (sd_z * (1 - pow(rho_yz,2)));
  
  double muBar = mean_x + t1 + t2;
  
  double sigmaBar = sqrt(GetSigmaBar(sd_x, rho_xy, rho_xz, rho_yz));
  
  if(! IS_FINITE(muBar)) {
    std::stringstream ss;
    ss << "In d3norm, muBar is not finite d3norm(x=" << x << ", y=" << y << ", z=" << z << ", mean_x=" << mean_x << ", mean_y=" << mean_y << ", mean_z=" << mean_z << ", sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z << ", rho_xy=" << rho_xy << ", rho_xz=" << rho_xz << ", rho_yz=" << rho_yz << ", returnLog=" << returnLog << ") t1=" << t1 << " t2=" << t2;
    HANDLE_GSL_ERROR(ss.str())
  }
  if(! IS_FINITE(sigmaBar)) {
    std::stringstream ss;
    ss << "In d3norm, sigmaBar is not finite d3norm(x=" << x << ", y=" << y << ", z=" << z << ", mean_x=" << mean_x << ", mean_y=" << mean_y << ", mean_z=" << mean_z << ", sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z << ", rho_xy=" << rho_xy << ", rho_xz=" << rho_xz << ", rho_yz=" << rho_yz << ", returnLog=" << returnLog << ")";
    HANDLE_GSL_ERROR(ss.str())
  }
  
  double retVal;
  
  double vals[] = {y, z}; double means[] = {mean_y, mean_z}; double sds[] = {sd_y, sd_z};
  
  if(returnLog) {
    retVal = sdist.dnorm(x, muBar, sigmaBar, returnLog) + sdist.dmnorm(vals, means, sds, &rho_yz, 2, returnLog);
    // + d2norm(y, z, mean_y, mean_z, sd_y, sd_z, rho_yz, returnLog);
    return retVal;
  }
  
  retVal = sdist.dnorm(x, muBar, sigmaBar, returnLog) * sdist.dmnorm(vals, means, sds, &rho_yz, 2, returnLog);
  // * d2norm(y, z, mean_y, mean_z, sd_y, sd_z, rho_yz, returnLog);
  
  return retVal;
}


/*
 void StatisticsHelper :: r2norm(double mean_x, double mean_y, double sd_x, double sd_y, double rho, double* x, double* y) {
 if(x == NULL || y == NULL) {
 HANDLE_GSL_ERROR("In function r2norm, The user is expected to allocate/deallocate the memory for randNum1 and randNum2 in calling function (i.e they cannot be NULL).")
 }
 
 gsl_ran_bivariate_gaussian(global_random_number_generator, sd_x, sd_y, rho, x, y);
 *x += mean_x;
 *y += mean_y;
 }
 
 double StatisticsHelper :: d2norm(double x, double y, double mean_x, double mean_y, double sd_x, double sd_y, double rho, bool returnLog) {
 if(returnLog) {
 //ln 2pi = 1.83784
 double t1 = (x - mean_x) / sd_x;
 double t2 = (y - mean_y) / sd_y;
 double t3 = 1 - pow(rho, 2);
 double t4 = -(pow(t1, 2) + pow(t2, 2) - 2*rho*t1*t2)/(2*t3);
 return t4 - (1.83784 + LOG(sd_x) + LOG(sd_y) + LOG(t3)/2 );
 }
 else {
 return gsl_ran_bivariate_gaussian_pdf((x - mean_x), (y - mean_y), sd_x, sd_y, rho);
 }
 }
 
 double StatisticsHelper :: conditional_r2norm(double mean_x, double mean_y, double sd_x, double sd_y, double rho, double y) {
 double newMean = mean_x + (sd_x / sd_y) * rho * (y - mean_y);
 double newSd = sqrt(1 - pow(rho,2)) * sd_x;
 return rnorm(newMean, newSd);
 }
 
 double StatisticsHelper :: conditional_r2tnorm(double mean_x, double mean_y, double sd_x, double sd_y, double rho, double y, double lower) {
 double newMean = mean_x + (sd_x / sd_y) * rho * (y - mean_y);
 double newSd = sqrt(1 - pow(rho,2)) * sd_x;
 return rtnorm(newMean, newSd, lower);
 }
 
 double StatisticsHelper :: conditional_r3norm(double mean_x, double mean_y, double mean_z, double sd_x, double sd_y, double sd_z, double rho_xy, double rho_xz, double rho_yz, double y, double z) {
 
 double muBar = GetMuBar(mean_x, mean_y, mean_z, sd_x, sd_y, sd_z, rho_xy, rho_xz, rho_yz, y, z);
 
 double sigmaBar = sqrt(GetSigmaBar(sd_x, rho_xy, rho_xz, rho_yz));
 
 if(rho_xy == 0 && rho_xz == 0 && rho_yz == 0 && (AbsoluteForDouble(mean_x - muBar) > 0.1 || AbsoluteForDouble(sd_x - sigmaBar) > 0.1)) {
 std::stringstream ss;
 ss << "In conditional_r3norm, Incorrect value when rho's are zero conditional_r3norm(y=" << y << ", z=" << z << ", mean_x=" << mean_x << ", mean_y=" << mean_y << ", mean_z=" << mean_z << ", sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z << ", rho_xy=" << rho_xy << ", rho_xz=" << rho_xz << ", rho_yz=" << rho_yz << ") -> [" << muBar << ", " << sigmaBar << "] " ;
 HANDLE_GSL_ERROR(ss.str())
 }
 
 if(gsl_finite(muBar) == 0) {
 std::stringstream ss;
 ss << "In conditional_r3norm, muBar is not finite conditional_r3norm(y=" << y << ", z=" << z << ", mean_x=" << mean_x << ", mean_y=" << mean_y << ", mean_z=" << mean_z << ", sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z << ", rho_xy=" << rho_xy << ", rho_xz=" << rho_xz << ", rho_yz=" << rho_yz << ") ";
 HANDLE_GSL_ERROR(ss.str())
 }
 if(gsl_finite(sigmaBar) == 0) {
 std::stringstream ss;
 ss << "In conditional_r3norm, sigmaBar is not finite conditional_r3norm(y=" << y << ", z=" << z << ", mean_x=" << mean_x << ", mean_y=" << mean_y << ", mean_z=" << mean_z << ", sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z << ", rho_xy=" << rho_xy << ", rho_xz=" << rho_xz << ", rho_yz=" << rho_yz << ")";
 HANDLE_GSL_ERROR(ss.str())
 }
 
 return rnorm(muBar, sigmaBar);
 }
 
 
 double StatisticsHelper :: conditional_r3tnorm(double mean_x, double mean_y, double mean_z, double sd_x, double sd_y, double sd_z, double rho_xy, double rho_xz, double rho_yz, double y, double z, double lower) {
 
 double muBar = GetMuBar(mean_x, mean_y, mean_z, sd_x, sd_y, sd_z, rho_xy, rho_xz, rho_yz, y, z);
 
 double sigmaBar = sqrt(GetSigmaBar(sd_x, rho_xy, rho_xz, rho_yz));
 
 if(rho_xy == 0 && rho_xz == 0 && rho_yz == 0 && (AbsoluteForDouble(mean_x - muBar) > 0.1 || AbsoluteForDouble(sd_x - sigmaBar) > 0.1)) {
 std::stringstream ss;
 ss << "In conditional_r3tnorm, Incorrect value when rho's are zero conditional_r3norm(y=" << y << ", z=" << z << ", mean_x=" << mean_x << ", mean_y=" << mean_y << ", mean_z=" << mean_z << ", sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z << ", rho_xy=" << rho_xy << ", rho_xz=" << rho_xz << ", rho_yz=" << rho_yz << ") -> [" << muBar << ", " << sigmaBar << "] " ;
 HANDLE_GSL_ERROR(ss.str())
 }
 
 if(gsl_finite(muBar) == 0) {
 std::stringstream ss;
 ss << "In conditional_r3norm, muBar is not finite conditional_r3norm(y=" << y << ", z=" << z << ", mean_x=" << mean_x << ", mean_y=" << mean_y << ", mean_z=" << mean_z << ", sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z << ", rho_xy=" << rho_xy << ", rho_xz=" << rho_xz << ", rho_yz=" << rho_yz << ")";
 HANDLE_GSL_ERROR(ss.str())
 }
 if(gsl_finite(sigmaBar) == 0) {
 std::stringstream ss;
 ss << "In conditional_r3norm, sigmaBar is not finite conditional_r3norm(y=" << y << ", z=" << z << ", mean_x=" << mean_x << ", mean_y=" << mean_y << ", mean_z=" << mean_z << ", sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z << ", rho_xy=" << rho_xy << ", rho_xz=" << rho_xz << ", rho_yz=" << rho_yz << ")";
 HANDLE_GSL_ERROR(ss.str())
 }
 
 return rtnorm(muBar, sigmaBar, lower);
 }
 
 */


double StatisticsHelper_myTempLogNorm2Function(const std::vector<double>& parameters) {
  double retVal;
  double myParameter = parameters[0];
  
  if(! IS_FINITE(myParameter)) {
    std::cout << "parameters[0]:" << myParameter;
    HANDLE_GSL_ERROR("Parameters passed to 2-normal probability function must be finite. Check maximization ")
  }
  
  if(SD_temp1 -> prevValueInitialized) {
    if(strcmp(SD_temp1 -> parameterType, "mean_x") == 0 || strcmp(SD_temp1 -> parameterType, "mean_y") == 0) {
      if(! IS_FINITE(SD_temp1 -> prevParamValue) || ! IS_FINITE(myParameter)) { HANDLE_GSL_ERROR("Incorrect parameters to normal 3 function1") }
      double prevPrior = (*(SD_temp1 -> LogPriorFunction))(SD_temp1 -> prevParamValue);
      double currPrior = (*(SD_temp1 -> LogPriorFunction))(myParameter);
      
      
      retVal = SD_temp1 -> prevReturnValue + currPrior - prevPrior + (myParameter - SD_temp1 -> prevParamValue) * (SD_temp1 -> prevConstantA1)
      - (pow(myParameter, 2) - pow(SD_temp1 -> prevParamValue, 2)) * (SD_temp1 -> prevConstantA2);
      
      if(! IS_FINITE(retVal)) {
        std::stringstream ss;
        ss << "In (Optimized) StatisticsHelper_myTempLogNorm2Function, the return value is infinite or NAN for the value " << myParameter  << " of " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      return retVal;
    }
    else if(strcmp(SD_temp1 -> parameterType, "sd_x") == 0 || strcmp(SD_temp1 -> parameterType, "sd_y") == 0) {
      if(! IS_FINITE(SD_temp1 -> prevParamValue) || ! IS_FINITE(myParameter)) { HANDLE_GSL_ERROR("Incorrect parameters to normal 3 function1") }
      double prevPrior = (*(SD_temp1 -> LogPriorFunction))(SD_temp1 -> prevParamValue);
      double currPrior = (*(SD_temp1 -> LogPriorFunction))(myParameter);
      
      double new1 = 1 / myParameter;
      double old1 = 1 / (SD_temp1 -> prevParamValue);
      
      assert((myParameter/(SD_temp1 -> prevParamValue)) > 0);
      retVal = SD_temp1 -> prevReturnValue + currPrior - prevPrior - (SD_temp1 -> numValues)*log(myParameter/(SD_temp1 -> prevParamValue))
      - (pow(new1, 2) - pow(old1, 2))*(SD_temp1 -> prevConstantA3) + (new1 - old1)*(SD_temp1 -> prevConstantA4);
      
      if(! IS_FINITE(retVal)) {
        std::stringstream ss;
        ss << "In (Optimized) StatisticsHelper_myTempLogNorm2Function, the return value is infinite or NAN for the value " << myParameter  << " of " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      return retVal;
    }
    else if(strcmp(SD_temp1 -> parameterType, "rho") == 0) {
      if(! IS_FINITE(SD_temp1 -> prevParamValue) || ! IS_FINITE(myParameter)) { HANDLE_GSL_ERROR("Incorrect parameters to normal 3 function1") }
      double prevPrior = (*(SD_temp1 -> LogPriorFunction))(SD_temp1 -> prevParamValue);
      double currPrior = (*(SD_temp1 -> LogPriorFunction))(myParameter);
      
      double new1 = 1 - pow(myParameter, 2);
      double old1 = 1 - pow(SD_temp1 -> prevParamValue, 2);
      assert((new1/old1) > 0);
      retVal = SD_temp1 -> prevReturnValue + currPrior - prevPrior - (SD_temp1 -> numValues)*log(new1/old1)/2
      + (1/(2*new1) - 1/(2*old1))*(SD_temp1 -> prevConstantA5) - (myParameter/new1 - (SD_temp1 -> prevParamValue)/old1)*(SD_temp1 -> prevConstantA6);
      
      if(! IS_FINITE(retVal)) {
        std::stringstream ss;
        ss << "In (Optimized) StatisticsHelper_myTempLogNorm2Function, the return value is infinite or NAN for the value " << myParameter  << " of " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      return retVal;
    }
  }
  
  double logProb = (*(SD_temp1 -> LogPriorFunction))(myParameter);
  if(! IS_FINITE(logProb)) { HANDLE_GSL_ERROR("The prior for the passed value is not finite") }
  
  double* meanx, *meany, *sdx, *sdy, *rhoxy;
  double* x = SD_temp1 -> x;
  double* y = SD_temp1 -> y;
  
  // Default
  meanx = SD_temp1 -> mean_x;
  meany = SD_temp1 -> mean_y;
  sdx = SD_temp1 -> sd_x;
  sdy = SD_temp1 -> sd_y;
  rhoxy = SD_temp1 -> rho_xy;
  
  SD_temp1 -> prevConstantA1 = 0; SD_temp1 -> prevConstantA2 = 0; SD_temp1 -> prevConstantA3 = 0;
  SD_temp1 -> prevConstantA4 = 0; SD_temp1 -> prevConstantA5 = 0; SD_temp1 -> prevConstantA6 = 0;
  
  
  
  StatisticsHelper sdist;
  if((SD_temp1 -> numValues) == 0) {
    HANDLE_GSL_ERROR("No data for StatisticsHelper_myTempLogNorm2Function. Sample from prior instead.")
  }
  
  if(strcmp(SD_temp1 -> parameterType, "mean_x") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double vals[] = {x[i], y[i]}; double means[] = {myParameter, meany[i]}; double sds[] = {sdx[i], sdy[i]};
      double myLogProb = sdist.dmnorm(vals, means, sds, &rhoxy[i], 2, true);
      // = sdist.d2norm(x[i], y[i], myParameter, meany[i], sdx[i], sdy[i], rhoxy[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNorm2Function, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      
      logProb += myLogProb;
      
      double t1 = (1-pow(rhoxy[i],2));
      SD_temp1 -> prevConstantA1 +=  x[i]/(pow(sdx[i],2) * t1) - (rhoxy[i] * (y[i] - meany[i]))/(sdx[i]*sdy[i]*t1);
      SD_temp1 -> prevConstantA2 += 1 / (2 * pow(sdx[i], 2) * t1);
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "mean_y") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double vals[] = {x[i], y[i]}; double means[] = {meanx[i], myParameter}; double sds[] = {sdx[i], sdy[i]};
      double myLogProb = sdist.dmnorm(vals, means, sds, &rhoxy[i], 2, true);
      // double myLogProb = sdist.d2norm(x[i], y[i], meanx[i], myParameter, sdx[i], sdy[i], rhoxy[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNorm2Function, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      
      logProb += myLogProb;
      
      double t1 = (1-pow(rhoxy[i],2));
      SD_temp1 -> prevConstantA1 +=  y[i]/(pow(sdy[i],2) * t1) - (rhoxy[i] * (x[i] - meanx[i]))/(sdx[i]*sdy[i]*t1);
      SD_temp1 -> prevConstantA2 += 1 / (2 * pow(sdy[i], 2) * t1);
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "sd_x") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double vals[] = {x[i], y[i]}; double means[] = {meanx[i], meany[i]}; double sds[] = {myParameter, sdy[i]};
      double myLogProb = sdist.dmnorm(vals, means, sds, &rhoxy[i], 2, true);
      // double myLogProb = sdist.d2norm(x[i], y[i], meanx[i], meany[i], myParameter, sdy[i], rhoxy[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNorm2Function, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      
      logProb += myLogProb;
      
      double t1 = (1-pow(rhoxy[i],2));
      SD_temp1 -> prevConstantA3 += pow(x[i] - meanx[i], 2)/(2*t1);
      SD_temp1 -> prevConstantA4 += (rhoxy[i] * (x[i] - meanx[i]) * (y[i] - meany[i])) / (t1*sdy[i]);
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "sd_y") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double vals[] = {x[i], y[i]}; double means[] = {meanx[i], meany[i]}; double sds[] = {sdx[i], myParameter};
      double myLogProb = sdist.dmnorm(vals, means, sds, &rhoxy[i], 2, true);
      // double myLogProb = sdist.d2norm(x[i], y[i], meanx[i], meany[i], sdx[i], myParameter, rhoxy[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNorm2Function, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      logProb += myLogProb;
      
      double t1 = (1-pow(rhoxy[i],2));
      SD_temp1 -> prevConstantA3 += pow(y[i] - meany[i], 2)/(2*t1);
      SD_temp1 -> prevConstantA4 += (rhoxy[i] * (y[i] - meany[i]) * (x[i] - meanx[i])) / (t1*sdx[i]);
      
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "rho") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double vals[] = {x[i], y[i]}; double means[] = {meanx[i], meany[i]}; double sds[] = {sdx[i], sdy[i]};
      double myLogProb = sdist.dmnorm(vals, means, sds, &myParameter, 2, true);
      // double myLogProb = sdist.d2norm(x[i], y[i], meanx[i], meany[i], sdx[i], sdy[i], myParameter, true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNorm2Function, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      logProb += myLogProb;
      
      double t1x = (x[i]-meanx[i])/sdx[i];
      double t1y = (y[i]-meany[i])/sdy[i];
      SD_temp1 -> prevConstantA5 += pow(t1x, 2) + pow(t1y, 2);
      SD_temp1 -> prevConstantA6 += t1x*t1y;
    }
  }
  else {
    std::stringstream ss;
    ss << "In StatisticsHelper_myTempLogNorm2Function, parameterType cannot be " << SD_temp1 -> parameterType;
    HANDLE_GSL_ERROR(ss.str())
  }
  
  
  if(! IS_FINITE(logProb)) {
    std::stringstream ss;
    ss << "In StatisticsHelper_myTempLogNorm2Function, the log probability is infinite or NAN where logProb from prior is " << (*(SD_temp1 -> LogPriorFunction))(myParameter) << " for the value " << myParameter  << " of " << SD_temp1 -> parameterType;
    HANDLE_GSL_ERROR(ss.str())
  }
  
  
  SD_temp1 -> prevParamValue = myParameter;
  SD_temp1 -> prevReturnValue = logProb;
  return logProb;
}


double StatisticsHelper :: rParameter2Norm(double* mean_x, double* mean_y, double* sd_x, double* sd_y, double* rho, double* x, double* y, unsigned int numValues, const char* parameterType, double initialValue) {
  // can be mean_x, mean_y, sd_x, sd_y, rho
  double left, right, maxima, argMax;
  enum TypeOfBoundsForOptimization howBounded;
  
  struct _TempDataStructureForNorm mySDTemp;
  mySDTemp.mean_x = mean_x;
  mySDTemp.mean_y = mean_y;
  mySDTemp.sd_x = sd_x;
  mySDTemp.sd_y = sd_y;
  mySDTemp.rho_xy = rho;
  mySDTemp.x = x;
  mySDTemp.y = y;
  mySDTemp.numValues = numValues;
  mySDTemp.parameterType = parameterType;
  // Set the global variable
  SD_temp1 = &mySDTemp;
  
  if(strcmp(parameterType, "mean_x") == 0 || strcmp(parameterType, "mean_y") == 0) {
    mySDTemp.LogPriorFunction = myLogIGPrior;
    left = -INT_MAX;
    right = INT_MAX;
    howBounded = SOFT_BOTH;
    // howBounded = HARD_BOTH_SIDES;
    if(numValues == 0) {
      // Sample from Inverse Gamma Prior
      return rinvgamma(1.0, 1.0);
    }
  }
  else if(strcmp(parameterType, "sd_x") == 0 || strcmp(parameterType, "sd_y") == 0) {
    mySDTemp.LogPriorFunction = myLogIGPrior;
    left = 0.001;
    right = INT_MAX;
    howBounded = HARD_LOWER;
    // howBounded = HARD_BOTH_SIDES;
    if(initialValue <= 0) { initialValue = DEFAULT_INITIAL_VAL_FOR_SD; }
    if(numValues == 0) {
      // Sample from Inverse Gamma Prior
      return rinvgamma(1.0, 1.0);
    }
  }
  else if(strcmp(parameterType, "rho") == 0) {
    mySDTemp.LogPriorFunction = myLogBetaPrior;
    left = -1 + 0.001; right = 1 - 0.001;
    howBounded = HARD_BOTH_SIDES;
    // initialValue = runif(-1 + 0.001, 1 - 0.001);
    if(numValues == 0) {
      // Sample from Beta Prior
      return NormalizeBetaToRho(rbeta(1.0, 1.0));
    }
  }
  else {
    std::stringstream ss;
    ss << "In rParameter3Norm, parameterType cannot be " << parameterType;
    HANDLE_GSL_ERROR(ss.str())
  }
  
  
  double originalLeft = left;
  double originalRight = right;
  double originalInitialValue = initialValue;
  
  SD_temp1 -> prevValueInitialized = false;
  std::vector<double> initialValues; initialValues.push_back(initialValue);
  double initialLogProb = (*StatisticsHelper_myTempLogNorm2Function)(initialValues);
  SD_temp1 -> prevValueInitialized = true;
  
  
  FindMultiModalMaximaInRange(StatisticsHelper_myTempLogNorm2Function, initialValue, left, right, &maxima, &argMax, howBounded);
  OneDimensionalOptimizer myOpt(StatisticsHelper_myTempLogNorm2Function, initialValues, 0.001, left, right, howBounded, 0);
  myOpt.FindTighterLRBounds(maxima, argMax, &left, &right, true);
  
  // std::cout << "Initial Log Prob:" << initialLogProb << " Maxima:" << maxima << " ";
  if(initialLogProb > maxima) {
    HANDLE_GSL_ERROR("Optimization failed sanity check. initial log-prob is greater than maxima")
  }
  if(left >= right) {
    std::stringstream ss;
    ss << "Incorrect bounding box by MaximizationRoutineValues:{ LR:[" << left << "," << right << "] }\n";
    HANDLE_GSL_ERROR(ss.str())
  }
  
#ifdef PRINT_LOG
  std::cout << "{InitVal:" << initialValue << "; BoundingBox:[" << left << ", " << right << "]; (Maxima:" << maxima << " Argmax:" << argMax << ")} ";
#endif
  
  double lowestVal, lowestArg;
  initialValues.clear(); initialValues.push_back(left);
  lowestVal = (*StatisticsHelper_myTempLogNorm2Function)(initialValues);
  initialValues.clear(); initialValues.push_back(right);
  double tempLVal = (*StatisticsHelper_myTempLogNorm2Function)(initialValues);
  lowestVal =  min(tempLVal, lowestVal);
  
  // Now start the rejection sampler
  // Two normalizing factor: nf1 and nf2 such that
  // exp(nf2 + nf1*maxima) = 1
  // exp(nf2 + nf1*lowestVal) = 0.001
  // Taking into account LOG(1) = 0 and LOG(0.001) = -6.907,
  // we get: nf1 = 6.907/(maxima - lowestVal) and nf2= -nf1*maxima
  double nf1 = 6.907/(maxima - lowestVal);
  double nf2= -nf1*maxima;
  unsigned int iter = 0;
  
  while(1) {
    double tempX = runif(left, right);
    initialValues.clear(); initialValues.push_back(tempX);
    double logProb = (*StatisticsHelper_myTempLogNorm2Function)(initialValues);
    double myProb = exp(nf2 + nf1*logProb);
    double tempY = runif(0, 1.0); // Since normalized
    if(myProb == 0) {
      std::cout << "*";
    }
    
    iter++;
    if(iter % 100000 == 0) {
      std::stringstream ss;
      ss << "Too many iterations in rejection sampling for " << parameterType << "\n";
      HANDLE_GSL_ERROR(ss.str())
    }
    
    if(myProb > 1 && (AbsoluteForDouble(argMax - tempX) <= 0.1 || AbsoluteForDouble(maxima - logProb) <= 0.1)) {
      // i.e Maxima is in close vicinity
      myProb = 0.99;
    }
    
    if(myProb < 0 || myProb > 1.1) {
      double tmp[200];
      std::cout << "\n(" << left << ", " << right << ")\n";
      for(int i = 0; i < 200; i++) {
        tmp[i] = runif(left, right);
        std::cout << tmp[i] << ", ";
      }
      std::cout << "\n\n";
      qsort(tmp, 200, sizeof(double), compare);
      
      std::cout << "\n\n";
      for(int i = 0; i < 200; i++) {
        std::cout << tmp[i] << ", ";
      }
      std::cout << "\n\n";
      
      for(int i = 0; i < 200; i++) {
        initialValues.clear(); initialValues.push_back(tmp[i]);
        double logProb1 = (*StatisticsHelper_myTempLogNorm2Function)(initialValues);
        
        std::cout << logProb1 << ", ";
        // std::cout << exp(nf2 + nf1*logProb1) << ", ";
        
      }
      std::cout << "\n\n";
      
      std::stringstream ss;
      ss << "Something wrong with maximization fn or probability fn of " << parameterType << " since calculated probability is " << myProb << " (logProb:" << logProb << ") for value " << tempX << " MaximizationRoutineValues:{ LR:[" << left << "," << right << "] Max-Arg:[" << maxima << "," << argMax << "] }\n";
      HANDLE_GSL_ERROR(ss.str())
    }
    
    if(tempY <= myProb) {
      if(tempX > 100000) {
        std::cout << "Warning: Sampled value for " << parameterType << " is " << tempX << " where initial Value is " << initialValue << " and box is [" << left << " " << argMax << " " << right << "] ";
      }
      SD_temp1 -> prevValueInitialized = false;
      return tempX;
    }
  }
}



double StatisticsHelper :: rbeta(double shape1, double shape2) {
  return gsl_ran_beta(global_random_number_generator, shape1, shape2);
}

double StatisticsHelper :: dbeta(double x, double shape1, double shape2, bool returnLog) {
  if(returnLog) {
    HANDLE_GSL_ERROR("In dbeta, log functionality not implemented yet")
    return GSL_NAN;
  }
  else {
    return gsl_ran_beta_pdf(x, shape1, shape2);
  }
}

double StatisticsHelper :: rgamma(double shape, double scale) {
  return gsl_ran_gamma(global_random_number_generator, shape, scale);
}

double StatisticsHelper :: dgamma(double x, double shape, double scale, bool returnLog) {
  if(returnLog) {
    // See http://en.wikipedia.org/wiki/Gamma_distribution
    double t1 = (shape - 1)*LOG(x);
    double t2 = (-x / scale);
    double t3 = shape*LOG(scale) + lgamma(shape);
    
    double retVal = t1 + t2 - t3;
    if(! IS_FINITE(retVal)) {
      std::stringstream ss;
      ss << "In function dgamma, the probability is not finite for parameters -> dgamma(" << x << ", " << shape << ", " << scale << ") ";
      HANDLE_GSL_ERROR(ss.str())
    }
    return retVal;
  }
  else {
    return gsl_ran_gamma_pdf(x, shape, scale);
  }
}

double StatisticsHelper :: rinvgamma(double shape, double scale) {
  double scaleInv = 1 / scale;
  if(! IS_FINITE(scaleInv)) {
    std::stringstream ss;
    ss << "In function rinvgamma, 1/scale is not finite where scale=" << scale;
    HANDLE_GSL_ERROR(ss.str())
  }
  double randNum1 = 1 / gsl_ran_gamma(global_random_number_generator, shape, scaleInv);
  if(! IS_FINITE(randNum1)) {
    std::stringstream ss;
    ss << "Error: In function rinvgamma, the return value is not finite for shape=" << shape << " and scale=" << scale;
    HANDLE_GSL_ERROR(ss.str())
  }
  return randNum1;
  
}

double StatisticsHelper :: dinvgamma(double x, double shape, double scale, bool returnLog) {
  if(returnLog) {
    if(x <= 0 || shape <= 0 || scale <= 0) {
      std::stringstream ss;
      ss << "In dinvgamma(log), Parameters to logdinvgamma cannot be negative or equal to zero where x=" << x << ", shape=" << shape << ", scale=" <<  scale;
      HANDLE_GSL_ERROR(ss.str())
      
    }
    // To avoid getting error when shape = 1 and lgamma(1) = -0 which is represented  as NaN
    double  t1 = lgamma(shape) == 0.0 ? 0.0 : lgamma(shape);
    
    double t2 = shape*LOG(scale);
    double t3 = (shape + 1)*LOG(x);
    double t4 = scale/x;
    double retVal = t2 + t1 - t3 - t4;
    if(! IS_FINITE(retVal)) {
      if(x > 0 && ! IS_FINITE(t1)) {
        std::stringstream ss;
        ss << "In dinvgamma(log), GSL cannot compute logGamma("<< shape <<")=" << t1 << " in logdinvgamma(x=" << x << ", shape=" << shape << ", scale=" <<  scale;
        HANDLE_GSL_ERROR(ss.str())
      }
      std::stringstream ss;
      ss << "In dinvgamma(log), logdinvgamma(x=" << x << ", shape=" << shape << ", scale=" <<  scale << "is infinite or NAN (i.e probability is zero)";
      HANDLE_GSL_ERROR(ss.str())
    }
    return retVal;
    
  }
  else {
    return gsl_ran_gamma_pdf(1/x, shape, 1/scale);
  }
}

double StatisticsHelper :: max(double a, double b) {
  if(a > b) {
    return a;
  }
  else {
    return b;
  }
}

double StatisticsHelper :: min(double a, double b) {
  if(a < b) {
    return a;
  }
  else {
    return b;
  }
}

int StatisticsHelper :: DoubleCompare(const void* a, const void* b) {
  if(*(double*)a  < *(double*)b) {
    return -1;
  }
  else if(*(double*)a  == *(double*)b) {
    return 0;
  }
  else {
    return 1;
  }
  // Works only for integers: return ( *(double*)a - *(double*)b );
}

double StatisticsHelper :: mean(double* array, int numOfElements) {
  int i = 0; double sum = 0;
  for(i = 0; i < numOfElements; i++) {
    sum += array[i];
  }
  return sum / numOfElements;
}

double StatisticsHelper :: variance(double* array, int numOfElements, double mean) {
  int i = 0; double sum = 0;
  for(i = 0; i < numOfElements; i++) {
    sum += (array[i] - mean) * (array[i] - mean);
  }
  return sum / numOfElements;
}

double StatisticsHelper :: lgamma(double x) {
  double  retVal = gsl_sf_lngamma(x);
  
  if(! IS_FINITE(retVal) && gsl_sf_lngamma(x) == 0.0) {
    // Since loggamma(1.0) = -0.0 --> nan
    retVal = 0.0;
  }
  
  if(! IS_FINITE(retVal)) {
    std::stringstream ss;
    ss << "Warning: loggamma(" << x << ") =" << retVal;
    HANDLE_GSL_ERROR(ss.str())
  }
  return retVal;
}




double StatisticsHelper_myTempLogNorm3Function(const std::vector<double>& parameters) {
  double retVal;
  double myParameter = parameters[0];
  
  
  if(! IS_FINITE(myParameter)) {
    // return -INT_MAX;
    std::cout << "parameters[0]:" << myParameter;
    HANDLE_GSL_ERROR("Parameters passed to 3-normal probability function must be finite. Check maximization ")
  }
  
  if(SD_temp1 -> prevValueInitialized) {
    
    if(strcmp(SD_temp1 -> parameterType, "mean_x") == 0 || strcmp(SD_temp1 -> parameterType, "mean_y") == 0 || strcmp(SD_temp1 -> parameterType, "mean_z") == 0) {
      if(! IS_FINITE(SD_temp1 -> prevParamValue) || ! IS_FINITE(myParameter)) { HANDLE_GSL_ERROR("Incorrect parameters to normal 3 function1") }
      double prevPrior = (*(SD_temp1 -> LogPriorFunction))(SD_temp1 -> prevParamValue);
      double currPrior = (*(SD_temp1 -> LogPriorFunction))(myParameter);
      
      // std::cout << "{" << SD_temp1 -> prevReturnValue  << " " <<  currPrior  << " " << prevPrior  << " " <<  myParameter << " " << SD_temp1 -> prevParamValue << " " << " " << SD_temp1 -> prevConstant1  << " " <<   SD_temp1 -> prevConstant2 << "}\n";
      
      retVal = SD_temp1 -> prevReturnValue + currPrior - prevPrior + (myParameter - SD_temp1 -> prevParamValue) *
      (SD_temp1 -> prevConstant1 - (myParameter + SD_temp1 -> prevParamValue) * SD_temp1 -> prevConstant2);
      if(! IS_FINITE(retVal)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, the return value is infinite or NAN for the value " << myParameter  << " of " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      return retVal;
    }
    else if(strcmp(SD_temp1 -> parameterType, "sd_x") == 0 || strcmp(SD_temp1 -> parameterType, "sd_y") == 0 || strcmp(SD_temp1 -> parameterType, "sd_z") == 0) {
      if(! IS_FINITE(SD_temp1 -> prevParamValue) || ! IS_FINITE(myParameter)) { HANDLE_GSL_ERROR("Incorrect parameters to normal 3 function2") }
      double prevPrior = (*(SD_temp1 -> LogPriorFunction))(SD_temp1 -> prevParamValue);
      double currPrior = (*(SD_temp1 -> LogPriorFunction))(myParameter);
      assert((SD_temp1 -> prevParamValue) > 0);
      assert(myParameter > 0);
      retVal = (SD_temp1 -> prevReturnValue) + currPrior - prevPrior - (SD_temp1 -> numValues)*(log(myParameter) - log(SD_temp1 -> prevParamValue));
      for(int i = 0; i < SD_temp1 -> numValues; i++) {
        double v1 = (SD_temp1 -> prevConstant3i[i] - myParameter*(SD_temp1 -> prevDeltaDashi[i])) / myParameter;
        double v2 = (SD_temp1 -> prevConstant3i[i] - (SD_temp1 -> prevParamValue)*(SD_temp1 -> prevDeltaDashi[i])) / (SD_temp1 -> prevParamValue);
        retVal -= (pow(v1, 2) - pow(v2, 2)) / (2*(SD_temp1 -> prevDeltaDoubleDashi[i]));
      }
      
      if(! IS_FINITE(retVal)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, the return value is infinite or NAN for the value " << myParameter  << " of " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      return retVal;
      
    }
    else if(strcmp(SD_temp1 -> parameterType, "rho_xy") == 0 || strcmp(SD_temp1 -> parameterType, "rho_xz") == 0 || strcmp(SD_temp1 -> parameterType, "rho_yz") == 0) {
      std::cout << "Rho is not optimized";
      /*
       The optimization formula and the code here don't match !!!
       if(gsl_finite(SD_temp1 -> prevParamValue) == 0 || gsl_finite(myParameter) == 0) { HANDLE_GSL_ERROR("Incorrect parameters to normal 3 function3") }
       double prevPrior = (*(SD_temp1 -> LogPriorFunction))(SD_temp1 -> prevParamValue);
       double currPrior = (*(SD_temp1 -> LogPriorFunction))(myParameter);
       double retVal = (SD_temp1 -> prevReturnValue) + currPrior - prevPrior;
       for(int i = 0; i < SD_temp1 -> numValues; i++) {
       double sigmaBarNew = SD_temp1 -> prevConstant6i[i] - (SD_temp1 -> prevConstant7i[i])*pow(myParameter,2) +  (SD_temp1 -> prevConstant8i[i])*myParameter;
       double sigmaBarOld = SD_temp1 -> prevConstant6i[i] - (SD_temp1 -> prevConstant7i[i])*pow(SD_temp1 -> prevParamValue,2) +  (SD_temp1 -> prevConstant8i[i])*(SD_temp1 -> prevParamValue);
       double vNew = SD_temp1 -> prevConstant4Dashi[i] - (SD_temp1 -> prevConstant5i[i])*myParameter;
       double vOld = SD_temp1 -> prevConstant4Dashi[i] - (SD_temp1 -> prevConstant5i[i])*(SD_temp1 -> prevParamValue);
       
       retVal += pow(vOld, 2) / (2*sigmaBarOld) - pow(vNew, 2) / (2*sigmaBarNew)
       - (LOG(sigmaBarNew) - LOG(sigmaBarOld)) / 2;
       
       }
       return retVal;
       */
    }
    else {
      HANDLE_GSL_ERROR("Incorrect parameter type passed to StatisticsHelper_myTempLogNorm3Function")
    }
    
  }
  
  double logProb = (*(SD_temp1 -> LogPriorFunction))(myParameter);
  
  if(! IS_FINITE(logProb)) { HANDLE_GSL_ERROR("The prior for the passed value is not finite") }
  
  double* meanx, *meany, *meanz, *sdx, *sdy, *sdz, *rhoxy, *rhoxz, *rhoyz;
  double* x = SD_temp1 -> x;
  double* y = SD_temp1 -> y;
  double* z = SD_temp1 -> z;
  
  // Default
  meanx = SD_temp1 -> mean_x;
  meany = SD_temp1 -> mean_y;
  meanz = SD_temp1 -> mean_z;
  sdx = SD_temp1 -> sd_x;
  sdy = SD_temp1 -> sd_y;
  sdz = SD_temp1 -> sd_z;
  rhoxy = SD_temp1 -> rho_xy;
  rhoxz = SD_temp1 -> rho_xz;
  rhoyz = SD_temp1 -> rho_yz;
  
  StatisticsHelper sdist;
  SD_temp1 -> prevConstant1 = 0;
  SD_temp1 -> prevConstant2 = 0;
  
  if(SD_temp1 -> prevConstant3i != NULL) {
    delete [](SD_temp1 -> prevConstant3i);
    delete [](SD_temp1 -> prevDeltaDashi);
    delete [](SD_temp1 -> prevDeltaDoubleDashi);
  }
  
  if(SD_temp1 -> prevConstant4Dashi != NULL) {
    delete [](SD_temp1 -> prevConstant4Dashi);  // = xi - prevConstant4i
    delete [](SD_temp1 -> prevConstant5i);
    delete [](SD_temp1 -> prevConstant6i);
    delete [](SD_temp1 -> prevConstant7i);
    delete [](SD_temp1 -> prevConstant8i);
  }
  
  if((SD_temp1 -> numValues) == 0) {
    HANDLE_GSL_ERROR("No data for StatisticsHelper_myTempLogNorm3Function. Sample from prior instead.")
  }
  
  
  if(strcmp(SD_temp1 -> parameterType, "sd_x") == 0 || strcmp(SD_temp1 -> parameterType, "sd_y") == 0 || strcmp(SD_temp1 -> parameterType, "sd_z") == 0) {
    SD_temp1 -> prevConstant3i = new double[SD_temp1 -> numValues];
    SD_temp1 -> prevDeltaDashi = new double[SD_temp1 -> numValues];
    SD_temp1 -> prevDeltaDoubleDashi = new double[SD_temp1 -> numValues];
  }
  
  if(strcmp(SD_temp1 -> parameterType, "rho_xy") == 0 || strcmp(SD_temp1 -> parameterType, "rho_xz") == 0 || strcmp(SD_temp1 -> parameterType, "rho_yz") == 0) {
    SD_temp1 -> prevConstant4Dashi = new double[SD_temp1 -> numValues]; // = xi - prevConstant4i
    SD_temp1 -> prevConstant5i = new double[SD_temp1 -> numValues];
    SD_temp1 -> prevConstant6i = new double[SD_temp1 -> numValues];
    SD_temp1 -> prevConstant7i = new double[SD_temp1 -> numValues];
    SD_temp1 -> prevConstant8i = new double[SD_temp1 -> numValues];
  }
  
  
  if(strcmp(SD_temp1 -> parameterType, "mean_x") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      // double muBar = GetMuBar(mean_x, mean_y, mean_z, sd_x, sd_y, sd_z, rho_xy, rho_xz, rho_yz, y, z);
      // double sigmaBar = sqrt(GetSigmaBar(sd_x, rho_xy, rho_xz, rho_yz));
      
      double myLogProb = d3norm(x[i], y[i], z[i], myParameter, meany[i], meanz[i], sdx[i], sdy[i], sdz[i], rhoxy[i], rhoxz[i], rhoyz[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      
      logProb += myLogProb;
      
      
      double x1 = x[i], y1 = y[i], z1 = z[i];
      double mu_y = meany[i], mu_z = meanz[i];
      double sd_x = sdx[i], sd_y = sdy[i], sd_z = sdz[i];
      double rho_xy = rhoxy[i], rho_xz = rhoxz[i], rho_yz = rhoyz[i];
      double deltai = GetDelta(sd_x, sd_y, sd_z, rho_xy, rho_xz, rho_yz, y1, z1, mu_y, mu_z);
      double sigmaBari = GetSigmaBar(sd_x, rho_xy, rho_xz, rho_yz);
      SD_temp1 -> prevConstant1 += (x1 - deltai) / sigmaBari;
      SD_temp1 -> prevConstant2 += 1 / (2*sigmaBari);
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "mean_y") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double myLogProb = d3norm(x[i], y[i], z[i], meanx[i], myParameter, meanz[i], sdx[i], sdy[i], sdz[i], rhoxy[i], rhoxz[i], rhoyz[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      
      logProb += myLogProb;
      
      
      double x1 = y[i], y1 = z[i], z1 = x[i];
      double mu_y = meanz[i], mu_z = meanx[i];
      double sd_x = sdy[i], sd_y = sdz[i], sd_z = sdx[i];
      double rho_xy = rhoyz[i], rho_xz = rhoxy[i], rho_yz = rhoxz[i];
      double deltai = GetDelta(sd_x, sd_y, sd_z, rho_xy, rho_xz, rho_yz, y1, z1, mu_y, mu_z);
      double sigmaBari = GetSigmaBar(sd_x, rho_xy, rho_xz, rho_yz);
      SD_temp1 -> prevConstant1 += (x1 - deltai) / sigmaBari;
      SD_temp1 -> prevConstant2 += 1 / (2*sigmaBari);
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "mean_z") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double myLogProb = d3norm(x[i], y[i], z[i], meanx[i], meany[i], myParameter, sdx[i], sdy[i], sdz[i], rhoxy[i], rhoxz[i], rhoyz[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      
      logProb += myLogProb;
      
      
      double x1 = z[i], y1 = x[i], z1 = y[i];
      double mu_y = meanx[i], mu_z = meany[i];
      double sd_x = sdz[i], sd_y = sdx[i], sd_z = sdy[i];
      double rho_xy = rhoxz[i], rho_xz = rhoyz[i], rho_yz = rhoxy[i];
      double deltai = GetDelta(sd_x, sd_y, sd_z, rho_xy, rho_xz, rho_yz, y1, z1, mu_y, mu_z);
      double sigmaBari = GetSigmaBar(sd_x, rho_xy, rho_xz, rho_yz);
      SD_temp1 -> prevConstant1 += (x1 - deltai) / sigmaBari;
      SD_temp1 -> prevConstant2 += 1 / (2*sigmaBari);
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "sd_x") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double myLogProb = d3norm(x[i], y[i], z[i], meanx[i], meany[i], meanz[i], myParameter, sdy[i], sdz[i], rhoxy[i], rhoxz[i], rhoyz[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      
      logProb += myLogProb;
      
      double x1 = x[i], y1 = y[i], z1 = z[i];
      double mu_x = meanx[i], mu_y = meany[i], mu_z = meanz[i];
      double sd_x = myParameter, sd_y = sdy[i], sd_z = sdz[i];
      double rho_xy = rhoxy[i], rho_xz = rhoxz[i], rho_yz = rhoyz[i];
      
      
      SD_temp1 -> prevConstant3i[i] = x1 - mu_x;
      SD_temp1 -> prevDeltaDashi[i] = ((rho_xy - rho_yz*rho_xz) * (y1 - mu_y)) / (sd_y*(1-pow(rho_yz,2))) + ((rho_xz - rho_yz*rho_xy) * (z1 - mu_z)) / (sd_z*(1-pow(rho_yz,2)));
      SD_temp1 -> prevDeltaDoubleDashi[i] = (1 - (pow(rho_xy,2) + pow(rho_xz,2) + pow(rho_yz,2) - 2*rho_xy*rho_xz*rho_yz)) / (1 - pow(rho_yz,2));
      
      
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "sd_y") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      
      double myLogProb = d3norm(x[i], y[i], z[i], meanx[i], meany[i], meanz[i], sdx[i], myParameter, sdz[i], rhoxy[i], rhoxz[i], rhoyz[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      logProb += myLogProb;
      
      double x1 = y[i], y1 = z[i], z1 = x[i];
      double mu_x = meany[i], mu_y = meanz[i], mu_z = meanx[i];
      double sd_x = myParameter, sd_y = sdz[i], sd_z = sdx[i];
      double rho_xy = rhoyz[i], rho_xz = rhoxy[i], rho_yz = rhoxz[i];
      
      SD_temp1 -> prevConstant3i[i] = x1 - mu_x;
      SD_temp1 -> prevDeltaDashi[i] = ((rho_xy - rho_yz*rho_xz) * (y1 - mu_y)) / (sd_y*(1-pow(rho_yz,2))) + ((rho_xz - rho_yz*rho_xy) * (z1 - mu_z)) / (sd_z*(1-pow(rho_yz,2)));
      SD_temp1 -> prevDeltaDoubleDashi[i] = (1 - (pow(rho_xy,2) + pow(rho_xz,2) + pow(rho_yz,2) - 2*rho_xy*rho_xz*rho_yz)) / (1 - pow(rho_yz,2));
      
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "sd_z") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double myLogProb = d3norm(x[i], y[i], z[i], meanx[i], meany[i], meanz[i], sdx[i], sdy[i], myParameter, rhoxy[i], rhoxz[i], rhoyz[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << myLogProb << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      logProb += myLogProb;
      
      double x1 = z[i], y1 = x[i], z1 = y[i];
      double mu_x = meanz[i], mu_y = meanx[i], mu_z = meany[i];
      double sd_x = myParameter, sd_y = sdx[i], sd_z = sdy[i];
      double rho_xy = rhoxz[i], rho_xz = rhoyz[i], rho_yz = rhoxy[i];
      
      SD_temp1 -> prevConstant3i[i] = x1 - mu_x;
      SD_temp1 -> prevDeltaDashi[i] = ((rho_xy - rho_yz*rho_xz) * (y1 - mu_y)) / (sd_y*(1-pow(rho_yz,2))) + ((rho_xz - rho_yz*rho_xy) * (z1 - mu_z)) / (sd_z*(1-pow(rho_yz,2)));
      SD_temp1 -> prevDeltaDoubleDashi[i] = (1 - (pow(rho_xy,2) + pow(rho_xz,2) + pow(rho_yz,2) - 2*rho_xy*rho_xz*rho_yz)) / (1 - pow(rho_yz,2));
      
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "rho_xy") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double myLogProb = d3norm(x[i], y[i], z[i], meanx[i], meany[i], meanz[i], sdx[i], sdy[i], sdz[i], myParameter, rhoxz[i], rhoyz[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      logProb += myLogProb;
      
      double x1 = x[i], y1 = y[i], z1 = z[i];
      double mu_x = meanx[i], mu_y = meany[i], mu_z = meanz[i];
      double sd_x = sdx[i], sd_y = sdy[i], sd_z = sdz[i];
      double rho_xy = myParameter, rho_xz = rhoxz[i], rho_yz = rhoyz[i];
      
      (SD_temp1 -> prevConstant4Dashi)[i] = x1 - (mu_x - (sd_x*rho_yz*rho_xz*(y1-mu_y)) / (sd_y*(1-pow(rho_yz, 2)))
                                                  + (sd_x*rho_xz*(z1-mu_z)) / (sd_z*(1-pow(rho_yz, 2)))); // = xi - prevConstant4i
      (SD_temp1 -> prevConstant5i)[i] = (sd_x*(y1 - mu_y)) / (sd_y*(1- pow(rho_yz, 2))) - (sd_x*rho_yz*(z1 - mu_z)) / (sd_z*(1- pow(rho_yz, 2)));
      (SD_temp1 -> prevConstant6i)[i] = (pow(sd_x, 2) * (1 - pow(rho_xz, 2) - pow(rho_yz, 2))) / (1 - pow(rho_yz, 2));
      (SD_temp1 -> prevConstant7i)[i] = pow(sd_x, 2) / (1 - pow(rho_yz, 2));
      (SD_temp1 -> prevConstant8i)[i] = (2 * pow(sd_x, 2) * rho_yz * rho_xz) / (1 - pow(rho_yz, 2));
      
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "rho_xz") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double myLogProb = d3norm(x[i], y[i], z[i], meanx[i], meany[i], meanz[i], sdx[i], sdy[i], sdz[i], rhoxy[i], myParameter, rhoyz[i], true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      logProb += myLogProb;
      
      double x1 = x[i], y1 = z[i], z1 = y[i];
      double mu_x = meanx[i], mu_y = meanz[i], mu_z = meany[i];
      double sd_x = sdx[i], sd_y = sdz[i], sd_z = sdy[i];
      double rho_xy = myParameter, rho_xz = rhoxy[i], rho_yz = rhoyz[i];
      
      (SD_temp1 -> prevConstant4Dashi)[i] = x1 - (mu_x - (sd_x*rho_yz*rho_xz*(y1-mu_y)) / (sd_y*(1-pow(rho_yz, 2)))
                                                  + (sd_x*rho_xz*(z1-mu_z)) / (sd_z*(1-pow(rho_yz, 2)))); // = xi - prevConstant4i
      (SD_temp1 -> prevConstant5i)[i] = (sd_x*(y1 - mu_y)) / (sd_y*(1- pow(rho_yz, 2))) - (sd_x*rho_yz*(z1 - mu_z)) / (sd_z*(1- pow(rho_yz, 2)));
      (SD_temp1 -> prevConstant6i)[i] = (pow(sd_x, 2) * (1 - pow(rho_xz, 2) - pow(rho_yz, 2))) / (1 - pow(rho_yz, 2));
      (SD_temp1 -> prevConstant7i)[i] = pow(sd_x, 2) / (1 - pow(rho_yz, 2));
      (SD_temp1 -> prevConstant8i)[i] = (2 * pow(sd_x, 2) * rho_yz * rho_xz) / (1 - pow(rho_yz, 2));
    }
  }
  else if(strcmp(SD_temp1 -> parameterType, "rho_yz") == 0) {
    for(int i = 0; i < (SD_temp1 -> numValues); i++) {
      double myLogProb = d3norm(x[i], y[i], z[i], meanx[i], meany[i], meanz[i], sdx[i], sdy[i], sdz[i], rhoxy[i], rhoxz[i], myParameter, true);
      if(! IS_FINITE(myLogProb)) {
        std::stringstream ss;
        ss << "In StatisticsHelper_myTempLogNormFunction, logProb is not finite for value " << myParameter << " of parameter: " << SD_temp1 -> parameterType;
        HANDLE_GSL_ERROR(ss.str())
      }
      logProb += myLogProb;
      
      double x1 = y[i], y1 = z[i], z1 = x[i];
      double mu_x = meany[i], mu_y = meanz[i], mu_z = meanx[i];
      double sd_x = sdy[i], sd_y = sdz[i], sd_z = sdx[i];
      double rho_xy = myParameter, rho_xz = rhoxy[i], rho_yz = rhoxz[i];
      
      (SD_temp1 -> prevConstant4Dashi)[i] = x1 - (mu_x - (sd_x*rho_yz*rho_xz*(y1-mu_y)) / (sd_y*(1-pow(rho_yz, 2)))
                                                  + (sd_x*rho_xz*(z1-mu_z)) / (sd_z*(1-pow(rho_yz, 2)))); // = xi - prevConstant4i
      (SD_temp1 -> prevConstant5i)[i] = (sd_x*(y1 - mu_y)) / (sd_y*(1- pow(rho_yz, 2))) - (sd_x*rho_yz*(z1 - mu_z)) / (sd_z*(1- pow(rho_yz, 2)));
      (SD_temp1 -> prevConstant6i)[i] = (pow(sd_x, 2) * (1 - pow(rho_xz, 2) - pow(rho_yz, 2))) / (1 - pow(rho_yz, 2));
      (SD_temp1 -> prevConstant7i)[i] = pow(sd_x, 2) / (1 - pow(rho_yz, 2));
      (SD_temp1 -> prevConstant8i)[i] = (2 * pow(sd_x, 2) * rho_yz * rho_xz) / (1 - pow(rho_yz, 2));
    }
  }
  else {
    std::stringstream ss;
    ss << "In StatisticsHelper_myTempLogNormFunction, parameterType cannot be " << SD_temp1 -> parameterType;
    HANDLE_GSL_ERROR(ss.str())
  }
  
  
  if(! IS_FINITE(logProb)) {
    std::stringstream ss;
    ss << "In StatisticsHelper_myTempLogNormFunction, the log probability is infinite or NAN where logProb from prior is " << (*(SD_temp1 -> LogPriorFunction))(myParameter) << " for the value " << myParameter  << " of " << SD_temp1 -> parameterType;
    HANDLE_GSL_ERROR(ss.str())
  }
  
  SD_temp1 -> prevReturnValue = logProb;
  SD_temp1 -> prevParamValue = myParameter;
  return logProb;
}



double NormalizeBetaToRho(double betaVal) {
  if(betaVal > 1 && betaVal < 1.01) {
    // Precision error - Ignore
    betaVal = 0.999;
  }
  else if(betaVal < 0 && betaVal > -0.01) {
    // Precision error - Ignore
    betaVal = 0.001;
  }
  
  if(betaVal > 1 || betaVal < 0) {
    HANDLE_GSL_ERROR("In NormalizeBetaToRho, Incorrect beta value")
  }
  
  return (betaVal * 2) - 1;
}


double NormalizeRhoToBeta(double rhoVal) {
  if(rhoVal > 1 && rhoVal < 1.01) {
    // Precision error - Ignore
    rhoVal = 0.999;
  }
  else if(rhoVal < -1 && rhoVal > -1.01) {
    // Precision error - Ignore
    rhoVal = -0.999;
  }
  
  if(rhoVal > 1 || rhoVal < -1) { HANDLE_GSL_ERROR("In NormalizeRhoToBeta, Incorrect rho value")  }
  return (rhoVal + 1) / 2;
}


double myLogBetaPrior(double x) {
  // You can change this to be read from file for better accuracy
  double val = gsl_ran_beta_pdf(NormalizeRhoToBeta(x), 1.0, 1.0);
  assert(val > 0);
  return log(val);
}

double myLogIGPrior(double x) {
  StatisticsHelper sd;
  
  // Since dinvgamma of negative values not possible, we will return very low value
  if(x <= 0) {
    return -INT_MAX;
  }
  
  return sd.dinvgamma(x, 1.0, 1.0, true);
}


double StatisticsHelper :: GetMuBar(double mean_x, double mean_y, double mean_z, double sd_x, double sd_y, double sd_z, double rho_xy, double rho_xz, double rho_yz, double y, double z) {
  if(sd_x <= 0 || sd_y <= 0 || sd_z <= 0) {
    std::stringstream ss;
    ss << "In d3GetMuBarnorm, sd cannot be less than or equal to 0 sd_x=" << sd_x << ", sd_y=" << sd_y << ", sd_z=" << sd_z;
    HANDLE_GSL_ERROR(ss.str())
  }
  
  double t1 = (sd_x * (rho_xy - rho_yz*rho_xz) * (y - mean_y)) / (sd_y * (1 - pow(rho_yz,2)));
  double t2 = (sd_x * (rho_xz - rho_yz*rho_xy) * (z - mean_z)) / (sd_z * (1 - pow(rho_yz,2)));
  
  return mean_x + t1 + t2;
  
}


// a*x^2 + b*x + c > 0
// lowerBound and upperBound should ideally be of array of size 2
void StatisticsHelper :: FindIntervalsForQuadraticInequalities(double a, double b, double c, double* lowerBound, double* upperBound, int* numIntervals) {
  double lower1, delta, upper1;
  if(lowerBound == NULL || upperBound == NULL) {
    HANDLE_GSL_ERROR("In FindIntervalsForQuadraticInequalities, lowerBound and upperBound should ideally be of array of size 3")
  }
  delta = pow(b, 2) - 4*a*c;
  
  if(delta < 0) {
    HANDLE_GSL_ERROR("In FindIntervalsForQuadraticInequalities, complex roots not supported")
  }
  if(a == 0) {
    HANDLE_GSL_ERROR("In FindIntervalsForQuadraticInequalities, only quadratic inequalities supported")
  }
  delta = sqrt(delta);
  
  lower1 = (-b - delta)/2*a;
  upper1 = (-b + delta)/2*a;
  
  // use delta as temp variable
  delta = lower1 < upper1 ? lower1 : upper1; // First store lower into temp
  upper1 = lower1 > upper1 ? lower1 : upper1;
  lower1 = delta;
  
  if((a*pow(lower1-0.1,2) + b*(lower1-0.1) + c) > 0) {
    // 2 intervals
    *numIntervals = 2;
    lowerBound[0] = -INFINITY;
    upperBound[0] = lower1;
    lowerBound[1] = upper1;
    upperBound[1] = INFINITY;
  }
  else {
    *numIntervals = 1;
    lowerBound[0] = lower1;
    upperBound[0] = upper1;
  }
}

void StatisticsHelper :: updateIntersection(double* intervalL, double* intervalR, int numIntervals, double lower, double upper) {
  if(numIntervals > 2) {
    HANDLE_GSL_ERROR("In updateIntersection, numIntervals > 2")
  }
  int isIntersected = 0, i;
  for(i = 0; i < numIntervals; i++) {
    if(intervalL[i] > upper || intervalR[i] < lower) {
      // NO intersection
    }
    else {
      isIntersected = 1;
      intervalL[i] = max(intervalL[i], lower);
      intervalR[i] = min(intervalR[i], upper);
    }
  }
  
  if(isIntersected == 0) {
    // HANDLE_GSL_ERROR("Warning: In updateIntersection, no intersection ")
    for(i = 0; i < numIntervals; i++) {
      // Instead of exiting return union
      intervalL[i] = min(intervalL[i], lower);
      intervalR[i] = max(intervalR[i], upper);
      
    }
  }
}


// Returns Intervals between [-1 + 0.0001, 1 - 0.0001] that satify the condition
// intervalL and R should be of size 2
int StatisticsHelper :: GetIntervalsThatSatisfiesInequalityConditions(double* rhoXZs, double* rhoYZs, int numRhos, double* intervalL, double* intervalR) {
  if(numRhos > 3) {
    HANDLE_GSL_ERROR("In GetIntervalsThatSatisfiesInequalityConditions, numRhos cannot be greater than 3")
  }
  if(intervalL == NULL || intervalR == NULL) {
    HANDLE_GSL_ERROR("In GetIntervalsThatSatisfiesInequalityConditions, interval cannot be NULL (and should be of size 6)")
  }
  
  double lowerBound[6], upperBound[6];
  
  int numIntervals[3], i, j;
  int retNumIntervals = 0;
  for(i = 0; i < numRhos; i++) {
    // Inequality equation with Chris regarding positive definite
    FindIntervalsForQuadraticInequalities(-1, (2*rhoXZs[i]*rhoYZs[i]), (1-pow(rhoXZs[i],2)-pow(rhoYZs[i],2)), &lowerBound[i*2], &upperBound[i*2], &numIntervals[i]);
    
    
    for(j = 0; j < numIntervals[i]; j++) {
      if(i == 0) {
        intervalL[retNumIntervals] = lowerBound[i*2+j];
        intervalR[retNumIntervals] = upperBound[i*2+j];
        retNumIntervals++;
      }
    }
  }
  
  // Find intersection of those intervals and uniform sampling in several ranges of interval
  for(i = 1; i < numRhos; i++) {
    for(j = 0; j < numIntervals[i]; j++) {
      updateIntersection(intervalL, intervalR, retNumIntervals, lowerBound[i*2+j], upperBound[i*2+j]);
    }
  }
  
  for(i = 0; i < retNumIntervals; i++) {
    if(intervalL[i] <= -1) {
      intervalL[i] = -1 + 0.001;
    }
    if(intervalR[i] >= 1) {
      intervalR[i] = 1 - 0.001;
    }
  }
  return retNumIntervals;
}


double StatisticsHelper :: SampleUniformlyFromIntervals(double* intervalL, double* intervalR, int numIntervals) {
  if(numIntervals == 1) {
    return runif(intervalL[0], intervalR[0]);
  }
  else if(numIntervals == 2) {
    double intervalSize = (intervalR[0] - intervalL[0]) + (intervalR[1] - intervalL[1]);
    double tempRho = runif(0.0001, intervalSize);
    if(tempRho < (intervalR[0] - intervalL[0])) {
      return tempRho + intervalL[0];
    }
    else {
      return (tempRho - (intervalR[0] - intervalL[0])) + intervalL[1];
    }
  }
  else {
    HANDLE_GSL_ERROR("In SampleUniformlyFromIntervals, numIntervals > 2 not supported")
  }
  
}

void GetRhos(double* rho_xz, double* rho_yz,  int numValues, double* retRhoXZ, double* retRhoYZ, int* numRetRhos) {
  *numRetRhos = 0;
  for(int i = 0; i < numValues; i++) {
    bool found = false;
    for(int j = 0; j < *numRetRhos; j++) {
      if(rho_xz[i] == retRhoXZ[j] && rho_yz[i] == retRhoYZ[j]) {
        found = true;
      }
    }
    
    if(!found && *numRetRhos >= 2) {
      HANDLE_GSL_ERROR("In GetRhos, Only 3 rhos allowed")
    }
    else if(!found) {
      retRhoXZ[(*numRetRhos)] = rho_xz[i]; retRhoYZ[(*numRetRhos)] = rho_yz[i];
      (*numRetRhos)++;
    }
  }
}

void FindMultiModalMaximaInRange(double(*myFunction)(const std::vector<double>& parameters), double initialValue, double left, double right, double* maxima, double* argMax, enum TypeOfBoundsForOptimization howBounded) {
  double maxima1, argMax1, left1, right1;
  
  std::vector<double> initialValues; initialValues.push_back(initialValue);
  OneDimensionalOptimizer* myOpt = new OneDimensionalOptimizer(myFunction, initialValues, 0.001, left, right, howBounded, 0);
  if(!(myOpt -> FindMaxima(&maxima1, &argMax1))) {
    HANDLE_GSL_ERROR("In rParameter2Norm, no maxima found")
  }
  *maxima = maxima1;
  *argMax = argMax1;
  delete myOpt;
  
  initialValues.clear(); initialValues.push_back(*argMax);
  myOpt = new OneDimensionalOptimizer(myFunction, initialValues, 0.001, left, right, howBounded, 0);
  if(myOpt -> FindMaxima(&maxima1, &argMax1)) {
    if(maxima1 > *maxima) {
      *maxima = maxima1;
      *argMax = argMax1;
    }
  }
  left1 = left; right1 = right;
  myOpt -> FindTighterLRBounds(*maxima, *argMax, &left1, &right1, true);
  // if(howBounded == HARD_LOWER) {
  
  // }
  delete myOpt;
  
  
  double newLeft = left1;
  double delta = (right1 - left1)/10;
  // std::cout << "\n{" << left1 << ", " << right1 << "}\n";
  for(int i = 0; i < 10; i++) {
    double newRight = newLeft + delta;
    
    initialValues.clear(); initialValues.push_back((newLeft + newRight)/2);
    
    myOpt = new OneDimensionalOptimizer(myFunction, initialValues, 0.001, newLeft, newRight, HARD_BOTH_SIDES, 0);
    if(myOpt -> FindMaxima(&maxima1, &argMax1)) {
      if(maxima1 > *maxima) {
        *maxima = maxima1;
        *argMax = argMax1;
      }
      // std::cout << "((" << newLeft << ", " << newRight << " [" << maxima1 << ", " << argMax1 << "] )) ";
    }
    delete myOpt;
    newLeft += delta;
  }
  // std::cout << "\n";
}

double StatisticsHelper :: rParameter3Norm(double* mean_x, double* mean_y, double* mean_z, double* sd_x, double* sd_y, double* sd_z, double* rho_xy, double* rho_xz, double* rho_yz, double* x, double* y, double* z, unsigned int numValues, const char* parameterType, double initialValue) {
  
  double left, right, maxima, argMax;
  enum TypeOfBoundsForOptimization howBounded;
  
  struct _TempDataStructureForNorm mySDTemp;
  mySDTemp.mean_x = mean_x;
  mySDTemp.mean_y = mean_y;
  mySDTemp.mean_z = mean_z;
  mySDTemp.sd_x = sd_x;
  mySDTemp.sd_y = sd_y;
  mySDTemp.sd_z = sd_z;
  mySDTemp.rho_xy = rho_xy;
  mySDTemp.rho_xz = rho_xz;
  mySDTemp.rho_yz = rho_yz;
  mySDTemp.x = x;
  mySDTemp.y = y;
  mySDTemp.z = z;
  mySDTemp.numValues = numValues;
  mySDTemp.parameterType = parameterType;
  // Set the global variable
  SD_temp1 = &mySDTemp;
  
  (SD_temp1 -> prevConstant3i) = NULL;
  (SD_temp1 -> prevDeltaDashi) = NULL;
  (SD_temp1 -> prevDeltaDoubleDashi) = NULL;
  (SD_temp1 -> prevConstant4Dashi) = NULL; // = xi - prevConstant4i
  (SD_temp1 -> prevConstant5i) = NULL;
  (SD_temp1 -> prevConstant6i) = NULL;
  (SD_temp1 -> prevConstant7i) = NULL;
  (SD_temp1 -> prevConstant8i) = NULL;
  
  if(strcmp(parameterType, "mean_x") == 0 || strcmp(parameterType, "mean_y") == 0 || strcmp(parameterType, "mean_z") == 0) {
    mySDTemp.LogPriorFunction = myLogIGPrior;
    left = -INT_MAX;
    right = INT_MAX;
    howBounded = SOFT_BOTH;
    // howBounded = HARD_BOTH_SIDES;
    if(numValues == 0) {
      // Sample from Inverse Gamma Prior
      return rinvgamma(1.0, 1.0);
    }
  }
  else if(strcmp(parameterType, "sd_x") == 0 || strcmp(parameterType, "sd_y") == 0 || strcmp(parameterType, "sd_z") == 0) {
    mySDTemp.LogPriorFunction = myLogIGPrior;
    left = 0.001;
    right = INT_MAX;
    howBounded = HARD_LOWER;
    // howBounded = HARD_BOTH_SIDES;
    if(initialValue <= 0) { initialValue = DEFAULT_INITIAL_VAL_FOR_SD; }
    if(numValues == 0) {
      // Sample from Inverse Gamma Prior
      return rinvgamma(1.0, 1.0);
    }
  }
  else if(strcmp(parameterType, "rho_xy") == 0 || strcmp(parameterType, "rho_xz") == 0 || strcmp(parameterType, "rho_yz") == 0) {
    mySDTemp.LogPriorFunction = myLogBetaPrior;
    left = -1 + 0.001; right = 1 - 0.001;
    howBounded = HARD_BOTH_SIDES;
    // initialValue = runif(-1 + 0.001, 1 - 0.001);
    if(numValues == 0) {
      // Sample from Beta Prior
      return NormalizeBetaToRho(rbeta(1.0, 1.0));
    }
  }
  else {
    std::stringstream ss;
    ss << "In rParameter3Norm, parameterType cannot be " << parameterType;
    HANDLE_GSL_ERROR(ss.str())
  }
  
  
  double originalLeft = left;
  double originalRight = right;
  double originalInitialValue = initialValue;
  
  SD_temp1 -> prevValueInitialized = false;
  std::vector<double> initialValues; initialValues.push_back(initialValue);
  double initialLogProb = (*StatisticsHelper_myTempLogNorm3Function)(initialValues);
  
  SD_temp1 -> prevValueInitialized = true;
  
  
  FindMultiModalMaximaInRange(StatisticsHelper_myTempLogNorm3Function, initialValue, left, right, &maxima, &argMax, howBounded);
  OneDimensionalOptimizer myOpt(StatisticsHelper_myTempLogNorm3Function, initialValues, 0.001, left, right, howBounded, 0);
  myOpt.FindTighterLRBounds(maxima, argMax, &left, &right, true);
  
  // std::cout << "Initial Log Prob:" << initialLogProb << " Maxima:" << maxima << " ";
  if(initialLogProb > maxima) {
    HANDLE_GSL_ERROR("Optimization failed sanity check. initial log-prob is greater than maxima")
  }
  
  // std::cout << "[2]\n";
  // Now find tighter left and right
  myOpt.FindTighterLRBounds(maxima, argMax, &left, &right, true);
  if(left >= right) {
    std::stringstream ss;
    ss << "Incorrect bounding box by MaximizationRoutineValues:{ LR:[" << left << "," << right << "] }\n";
    HANDLE_GSL_ERROR(ss.str())
  }
  // std::cout << "[3]\n";
  
#ifdef PRINT_LOG
  std::cout << "{InitVal:" << initialValue << "; BoundingBox:[" << left << ", " << right << "]; (Maxima:" << maxima << " Argmax:" << argMax << ")} ";
#endif
  
  // std::cout << "[3] [" << left << ", " << argMax << ", " << right << "]\n";
  
  double lowestVal, lowestArg;
  initialValues.clear(); initialValues.push_back(left);
  lowestVal = (*StatisticsHelper_myTempLogNorm3Function)(initialValues);
  initialValues.clear(); initialValues.push_back(right);
  double tempLVal = (*StatisticsHelper_myTempLogNorm3Function)(initialValues);
  lowestVal =  min(tempLVal, lowestVal);
  
  /*
   
   std::cout << "\nMinOpt)";
   OneDimensionalOptimizer myOpt3(StatisticsHelper_myTempLogNorm3Function, initialValues, 0.001, left, right, howBounded, 0);
   if(!myOpt3.FindMinima(&lowestVal, &lowestArg)) {
   HANDLE_GSL_ERROR("In rParameter2Norm, no minima found")
   }
   */
  
  
  if(lowestVal > maxima) {
    double tmp[200];
    std::cout << "\n(" << left << ", " << right << ")\n";
    for(int i = 0; i < 200; i++) {
      tmp[i] = runif(left, right);
      std::cout << tmp[i] << ", ";
    }
    std::cout << "\n\n";
    qsort(tmp, 200, sizeof(double), compare);
    
    std::cout << "\n\n";
    for(int i = 0; i < 200; i++) {
      std::cout << tmp[i] << ", ";
    }
    std::cout << "\n\n";
    
    for(int i = 0; i < 200; i++) {
      initialValues.clear(); initialValues.push_back(tmp[i]);
      double logProb1 = (*StatisticsHelper_myTempLogNorm3Function)(initialValues);
      
      std::cout << logProb1 << ", ";
      // std::cout << exp(nf2 + nf1*logProb1) << ", ";
      
    }
    std::cout << "\n\n";
    
    
    initialValues.clear(); initialValues.push_back(lowestArg);
    std::cout << "\nCheck lowestVal with " << (*StatisticsHelper_myTempLogNorm3Function)(initialValues);
    initialValues.clear(); initialValues.push_back(argMax);
    std::cout << "\nCheck maxima with " << (*StatisticsHelper_myTempLogNorm3Function)(initialValues);
    
    
    std::cout << "\nLowestval: " << lowestVal << "(" << lowestArg << "), Maxima: " << maxima << "(" << argMax << ")";
    HANDLE_GSL_ERROR("lowest val is greater than maxima")
  }
  
  // std::cout << "(MinOpt\n";
  
  double intervalL[2], intervalR[2]; int numIntervals;
  if(strcmp(SD_temp1 -> parameterType, "rho_xy") == 0 || strcmp(SD_temp1 -> parameterType, "rho_xz") == 0 || strcmp(SD_temp1 -> parameterType, "rho_yz") == 0) {
    double retRhoXZ[3]; double retRhoYZ[3]; int numRhos;
    
    if(strcmp(SD_temp1 -> parameterType, "rho_xy") == 0) {
      GetRhos(rho_xz, rho_yz, numValues, retRhoXZ, retRhoYZ, &numRhos);
    }
    else if(strcmp(SD_temp1 -> parameterType, "rho_xz") == 0) {
      GetRhos(rho_xy, rho_yz, numValues, retRhoXZ, retRhoYZ, &numRhos);
    }
    else if(strcmp(SD_temp1 -> parameterType, "rho_yz") == 0) {
      GetRhos(rho_xy, rho_xz, numValues, retRhoXZ, retRhoYZ, &numRhos);
    }
    numIntervals = GetIntervalsThatSatisfiesInequalityConditions(retRhoXZ, retRhoYZ, numRhos, intervalL, intervalR);
  }
  
  // Now start the rejection sampler
  // Two normalizing factor: nf1 and nf2 such that
  // exp(nf2 + nf1*maxima) = 1
  // exp(nf2 + nf1*lowestVal) = 0.001
  // Taking into account LOG(1) = 0 and LOG(0.001) = -6.907,
  // we get: nf1 = 6.907/(maxima - lowestVal) and nf2= -nf1*maxima
  double nf1 = 6.907/(maxima - lowestVal);
  double nf2= -nf1*maxima;
  
  unsigned int iter = 0;
  
  // std::cout <<  "NF:" << nf1 << ", " << nf2 << ", lowestVal: " << lowestVal  << " Box is [" << left << " " << argMax << "(" << maxima << ") " << right << "(";
  
  
  while(1) {
    double tempX = runif(left, right);
    if(strcmp(SD_temp1 -> parameterType, "rho_xy") == 0 || strcmp(SD_temp1 -> parameterType, "rho_xz") == 0 || strcmp(SD_temp1 -> parameterType, "rho_yz") == 0) {
      // tempX = SampleUniformlyFromIntervals(intervalL, intervalR, numIntervals);
      
      double r1, r2, r3;
      if(strcmp(SD_temp1 -> parameterType, "rho_xy") == 0) { r1 = tempX; r2 = rho_xz[0]; r3 = rho_yz[0]; }
      else if(strcmp(SD_temp1 -> parameterType, "rho_xz") == 0) { r1 = tempX; r2 = rho_yz[0]; r3 = rho_xy[0]; }
      else if(strcmp(SD_temp1 -> parameterType, "rho_yz") == 0) { r1 = tempX; r2 = rho_xy[0]; r3 = rho_xz[0]; }
      if(AreRhosValidForTriVariateNormal(r1, r2, r3) != 1) {
        // HANDLE_GSL_ERROR("Since you are sampling using inequality condition, sampled rho should always be valid") -> OR Logic
        continue;
      }
    }
    
    
    initialValues.clear(); initialValues.push_back(tempX);
    double logProb = (*StatisticsHelper_myTempLogNorm3Function)(initialValues);
    double myProb = exp(nf2 + nf1*logProb);
    
    double tempY = runif(0, 1.0); // Since normalized
    if(myProb == 0) {
      std::cout << "*";
    }
    
    iter++;
    if(iter % 100000 == 0) {
      std::stringstream ss;
      ss << "Too many iterations in rejection sampling for " << parameterType << "\n";
      HANDLE_GSL_ERROR(ss.str())
    }
    
    if(myProb > 1 && (AbsoluteForDouble(argMax - tempX) <= 0.1 || AbsoluteForDouble(maxima - logProb) <= 0.1)) {
      // i.e Maxima is in close vicinity
      myProb = 0.99;
    }
    
    if(myProb < 0 || myProb > 1.1) {
      double tmp[200];
      std::cout << "\n(" << left << ", " << right << ")\n";
      for(int i = 0; i < 200; i++) {
        tmp[i] = runif(left, right);
        std::cout << tmp[i] << ", ";
      }
      std::cout << "\n\n";
      qsort(tmp, 200, sizeof(double), compare);
      
      std::cout << "\n\n";
      for(int i = 0; i < 200; i++) {
        std::cout << tmp[i] << ", ";
      }
      std::cout << "\n\n";
      
      for(int i = 0; i < 200; i++) {
        initialValues.clear(); initialValues.push_back(tmp[i]);
        double logProb1 = (*StatisticsHelper_myTempLogNorm3Function)(initialValues);
        
        std::cout << logProb1 << ", ";
        // std::cout << exp(nf2 + nf1*logProb1) << ", ";
        
      }
      std::cout << "\n\n";
      
      std::stringstream ss;
      ss << "Something wrong with maximization fn or probability fn of " << parameterType << " since calculated probability is " << myProb << " (logProb:" << logProb << ") for value " << tempX << " MaximizationRoutineValues:{ LR:[" << left << "," << right << "] Max-Arg:[" << maxima << "," << argMax << "] }\n";
      HANDLE_GSL_ERROR(ss.str())
    }
    
    if(tempY <= myProb) {
      SD_temp1 -> prevValueInitialized = false;
      if(SD_temp1 -> prevConstant3i != NULL) {
        delete [](SD_temp1 -> prevConstant3i);
        delete [](SD_temp1 -> prevDeltaDashi);
        delete [](SD_temp1 -> prevDeltaDoubleDashi);
      }
      if(SD_temp1 -> prevConstant4Dashi != NULL) {
        delete [](SD_temp1 -> prevConstant4Dashi);  // = xi - prevConstant4i
        delete [](SD_temp1 -> prevConstant5i);
        delete [](SD_temp1 -> prevConstant6i);
        delete [](SD_temp1 -> prevConstant7i);
        delete [](SD_temp1 -> prevConstant8i);
      }
      // std::cout << "[4 - " << tempX << "]\n";
      if(tempX > 100000) {
        std::cout << "Warning: Sampled value for " << parameterType << " is " << tempX << " where initial Value is " << initialValue << " and box is [" << left << " " << argMax << " " << right << "] ";
      }
      
      return tempX;
    }
  }
}

double StatisticsHelper :: AbsoluteForDouble(double val) {
  if(val < 0) {
    return -val;
  }
  return val;
}

double OneDimensionalOptimizer :: AbsoluteForDouble(double val) {
  if(val < 0) {
    return -val;
  }
  return val;
}

double AbsoluteForDouble(double val) {
  if(val < 0) {
    return -val;
  }
  return val;
}

int StatisticsHelper :: AreRhosValidForTriVariateNormal(double rhoXY, double rhoYZ, double rhoXZ) {
  gsl_matrix* covarianceMatrix = gsl_matrix_alloc(3, 3);
  int retVal;
  
  gsl_matrix_set(covarianceMatrix, 0, 0, 1);
  gsl_matrix_set(covarianceMatrix, 1, 1, 1);
  gsl_matrix_set(covarianceMatrix, 2, 2, 1);
  gsl_matrix_set(covarianceMatrix, 0, 1, rhoXY);
  gsl_matrix_set(covarianceMatrix, 1, 0, rhoXY);
  gsl_matrix_set(covarianceMatrix, 0, 2, rhoXZ);
  gsl_matrix_set(covarianceMatrix, 2, 0, rhoXZ);
  gsl_matrix_set(covarianceMatrix, 1, 2, rhoYZ);
  gsl_matrix_set(covarianceMatrix, 2, 1, rhoYZ);
  
  MatrixHelper mHelper;
  
  if(mHelper.isPositiveDefinite(covarianceMatrix, 3, 3) == 0) {
    retVal = 0;
  }
  else {
    retVal = 1;
  }
  
  gsl_matrix_free(covarianceMatrix);
  
  return retVal;
}

/*
// Returns 0 if A is not positive definite
int StatisticsHelper :: IsMatrixPositiveDefinite(gsl_matrix* A) {
  gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
  int errCode = gsl_linalg_cholesky_decomp(A);
  gsl_set_error_handler(old_handler);
  
  if(errCode == GSL_EDOM) {
    return 0;
  }
  else {
    return 1;
  }
}
 */


double OneDimensionalOptimizer :: FindRoot(double rhsValue, double arg_low, double arg_high, double tolerance) {
  int iter = 0, max_iter = 100;
  const gsl_root_fsolver_type* T;
  gsl_root_fsolver* s;
  int status;
  
  gsl_function F;
  F.function = &myTransformedFunForGSL;
  struct _TempDataStructureForGSL tempDS;
  tempDS.ptrParams = &initialValues;
  tempDS.myFunction = f;
  tempDS.paramIndex = parameterIndex;
  tempDS.doMinimization = false;
  F.params = &tempDS;
  
  T = gsl_root_fsolver_brent;
  // T = gsl_root_fsolver_bisection;
  s = gsl_root_fsolver_alloc(T);
  gsl_root_fsolver_set(s, &F, arg_low, arg_high);
  
  do {
    iter++;
    gsl_root_fsolver_iterate(s);
    arg_low = gsl_root_fsolver_x_lower(s);
    arg_high = gsl_root_fsolver_x_upper(s);
    status = gsl_root_test_residual(rhsValue, tolerance);
    
    if(AbsoluteForDouble(arg_high - arg_low) < 0.00001) {
      double leftVal = myTransformedFunForGSL(arg_low, (void*)&tempDS);
      double rightVal = myTransformedFunForGSL(arg_high, (void*)&tempDS);
      std::cout << "\n>>" << leftVal << " < " << rhsValue << " < " << rightVal << "<<\n";
      exit(1);
    }
    
  }
  while(status == GSL_CONTINUE && iter < max_iter);
  
  gsl_root_fsolver_free(s);
  
  if(status != GSL_SUCCESS) {
    HANDLE_GSL_ERROR("Couldnot find root")
  }
  return gsl_root_fsolver_root(s);
}

double OneThousandth(double maxima, double* precision, bool isLog) {
  if(isLog) {
    *precision = 0.001;
    return maxima - 6.91;
  }
  else {
    HANDLE_GSL_ERROR("OneThousandth only works for log probability functions")
  }
  return 0;
}

bool OneDimensionalOptimizer :: BinarySearch(double arg_low, double val_low, double arg_high, double val_high, double checkValue, double tolerance, double* retVal, unsigned int iter) {
  if(AbsoluteForDouble(val_low - checkValue) < tolerance) {
    *retVal = arg_low;
    return true;
  }
  if(AbsoluteForDouble(val_high - checkValue) < tolerance) {
    *retVal = arg_high;
    return true;
  }
  
  if((val_low <= checkValue && checkValue <= val_high) || (val_low >= checkValue && checkValue >= val_high)) {
    // continue since checkVal is in the interval
    if(AbsoluteForDouble(arg_high - arg_low) < 0.0001) {
      // Very small search interval and checkValue is in the interval
      *retVal = (arg_low + arg_high) / 2;
      return true;
    }
    else if(iter >= 100) {
      *retVal = (arg_low + arg_high) / 2;
      return true;
    }
  }
  else { return false; }
  
  // Do this recursively
  double arg_middle = (arg_low + arg_high) / 2;
  initialValues[parameterIndex] = arg_middle;
  if(! IS_FINITE(arg_middle)) {
    HANDLE_GSL_ERROR("In Binary Search, incorrect argument")
  }
  
  double val_middle = (*f)(initialValues);
  
  //std::cout << "\n[" << arg_low << ", " << arg_middle << ", " << arg_high << "] {" << val_low << ", " << val_middle << ", " << val_high << "}";
  
  iter++;
  
  // Search left, then right
  if(BinarySearch(arg_low, val_low, arg_middle, val_middle, checkValue, tolerance, retVal, iter)) {
    return true;
  }
  else if(BinarySearch(arg_middle, val_middle, arg_high, val_high, checkValue, tolerance, retVal, iter)) {
    return true;
  }
  
  HANDLE_GSL_ERROR("No solution found for Binary search")
  return false;
}

void OneDimensionalOptimizer :: FindTighterLRBounds(double maxima, double argMax, double* lower, double* upper, bool isLog) {
  double arg, delta, precision;
  double prevArg;
  double checkVal = OneThousandth(maxima, &precision, isLog);
  
  //std::cout << "\n CheckVal: " << checkVal;
  
  double originalLower = *lower;
  double originalUpper = *upper;
  
  time_t seconds = time(NULL);
  // Find left hand side tigher bound
  // std::cout << "[1: Original[" << *lower << ", " << *upper << "]: >";
  delta = 0.1;
  arg = argMax - delta;
  do {
    prevArg = arg;
    delta *= 2; arg = argMax - delta;
    initialValues[parameterIndex] = arg;
    if((howBounded == HARD_BOTH_SIDES || howBounded == HARD_LOWER) && l > initialValues[parameterIndex]) {
      arg = l;
      break;
    }
    if(! IS_FINITE(delta) || ! IS_FINITE(arg) || ! IS_FINITE((*f)(initialValues)))  {
      arg = prevArg;
      break;
    }
    
    // std:: cout << "[" << initialValues[parameterIndex] << " " << (*f)(initialValues) << "]";
    
  }
  while((*f)(initialValues) > checkVal);
  *lower = arg;
  // std::cout << "[2: " << *lower << " >";
  
  if(*lower < originalLower) {
    *lower = originalLower;
  }
  
  // std:: cout << "\n" << checkVal << " (" << arg << "," << (*f)(initialValues) << ") (" << argMax << "," << maxima << ") " << precision << "\n";
  // *lower = FindRoot(checkVal, arg, argMax, precision);
  if((howBounded == HARD_BOTH_SIDES || howBounded == HARD_LOWER) && l == *lower) {
    // Lower bound is fixed
  }
  else {
    initialValues[parameterIndex] = arg;
    if(!BinarySearch(arg, (*f)(initialValues), argMax, maxima, checkVal, precision, lower, 0)) {
      if(howBounded == HARD_BOTH_SIDES || howBounded == HARD_LOWER) {
        *lower = l;
      }
      else {
        *lower = arg;
        // std::cout << "Arg-Maxima[" << argMax << "," << maxima << "] Arg:" << arg << " CheckVal:" << checkVal << " l:" << l << "\n";
        // HANDLE_GSL_ERROR("No solution found after binary search1")
      }
    }
  }
  if(*lower < originalLower) {
    *lower = originalLower;
  }
  
  // std::cout << "[3: " << *lower << " >";
  
  // Find right hand side tigher bound
  delta = 0.1;
  arg = argMax + delta;
  do {
    prevArg = arg;
    delta *= 2; arg = argMax + delta;
    initialValues[parameterIndex] = arg;
    if((howBounded == HARD_BOTH_SIDES || howBounded == HARD_UPPER) && r < initialValues[parameterIndex]) {
      arg = r;
      break;
    }
    if(! IS_FINITE(delta) || ! IS_FINITE(arg) || ! IS_FINITE((*f)(initialValues)))  {
      arg = prevArg;
      break;
    }
    //std:: cout << "[" << initialValues[parameterIndex] << " " << (*f)(initialValues) << "]";
  }
  while((*f)(initialValues) > checkVal);
  *upper = arg;
  // std::cout << "[4.1: " << *upper << " >";
  
  // *upper = FindRoot(checkVal, argMax, arg, precision);
  if(*upper > originalUpper) {
    *upper = originalUpper;
  }
  
  if((howBounded == HARD_BOTH_SIDES || howBounded == HARD_UPPER) && r == *upper) {
    // Upper bound is fixed
  }
  else {
    initialValues[parameterIndex] = arg;
    if(!BinarySearch(argMax, maxima, arg, (*f)(initialValues), checkVal, precision, upper, 0)) {
      if(howBounded == HARD_BOTH_SIDES || howBounded == HARD_UPPER) {
        *upper = r;
      }
      else {
        *upper = arg;
        // std::cout << "Arg-Maxima[" << argMax << "," << maxima << "] Arg:" << arg << " CheckVal:" << checkVal << " r:" << r << "\n";
        // HANDLE_GSL_ERROR("No solution found after binary search2")
      }
    }
  }
  if(*upper > originalUpper) {
    *upper = originalUpper;
  }
  
  // std::cout << "[5: " << *upper << " >";
  
  // initialValues[parameterIndex] = *lower;
  // std::cout <<  "{LB:" << *lower << " " << (*f)(initialValues) << " (Max:" << argMax << "," << maxima << ") RB:" << *upper;
  // initialValues[parameterIndex] = *upper;
  // std::cout << " "  <<(*f)(initialValues) << " " << (time(NULL) - seconds) << "}";
}

// -----------------------------------------------------------------------------------------------

// All matrices are stored row-wise

double MatrixHelper :: det(double* m, int order) {
  assert(order >= 2);
  if(order == 2) {
    return m[0]*m[3] - m[1]*m[2];
  }
  else if(order == 3) {
    return m[0]*m[4]*m[8] + m[1]*m[5]*m[6] + m[2]*m[3]*m[7] - (m[0]*m[5]*m[7] + m[1]*m[3]*m[8] + m[2]*m[4]*m[6]);
  }
  else {
    HANDLE_GSL_ERROR("Determinant of order > 3 is not supported.")
  }
}

#define FILL_INV(a, b, c, d, indx) { \
m1[0] = m[a]; m1[1] = m[b]; m1[2] = m[c]; m1[3] = m[d];  \
inv[indx] = det(m1, 2) / div; \
}
void MatrixHelper :: inv(double* m, double* inv, int order) {
  assert(order == 2 || order == 3);
  
  // Why I am not supporting order > 3 ?
  // See post: http://www.johndcook.com/blog/2010/01/19/dont-invert-that-matrix
  // I especially liked the comment: "To put it another way, pretend you have a software object AInverse. You can’t access the inverse directly, but AInverse has methods on it to let you do whatever you want: MultiplyByVector, GetDeterminant, etc. But when you look inside AInverse, there’s no inverse inside, just code that acts as if there were an inverse inside."
  
  double div = det(m, order);
  if(div == 0) { HANDLE_GSL_ERROR("Determinant of matrix is zero, hence inverse is not defined.") }
  
  if(order == 2) {
    inv[0] = m[3] / div;
    inv[1] = - m[1] / div;
    inv[2] = - m[2] / div;
    inv[3] = m[0] / div;
   }
  else if(order == 3) {
    double m1[4];
    
    // See http://stackoverflow.com/questions/983999/simple-3x3-matrix-inverse-code-c
    FILL_INV(4, 5, 7, 8, 0)
    FILL_INV(2, 1, 8, 7, 1)
    FILL_INV(1, 2, 4, 5, 2)
    FILL_INV(5, 3, 8, 6, 3)
    FILL_INV(0, 2, 6, 8, 4)
    FILL_INV(2, 0, 5, 3, 5)
    FILL_INV(3, 4, 6, 7, 6)
    FILL_INV(1, 0, 7, 6, 7)
    FILL_INV(0, 1, 3, 4, 8)
  }
  
}

void MatrixHelper :: inv_LU_decomposition(const double *matrix, double *retVal, int order) {
  int s;
  
  gsl_permutation *p = gsl_permutation_alloc(order);
  
  gsl_matrix * temp = gsl_matrix_calloc(order, order);
  convert_rowWise_to_gslMatrix(matrix, temp, order, order);
  
  gsl_linalg_LU_decomp(temp, p, &s);
  gsl_linalg_LU_invert(temp, p, temp);
  
  convert_gslMatrix_to_rowWise(temp, retVal, order, order);
  
  gsl_permutation_free(p);
  gsl_matrix_free(temp);
}

void MatrixHelper :: inv_cholesky_decomposition(const double *matrix, double *retVal, int order) {
  // Note m should be positive definite
  gsl_matrix * temp = gsl_matrix_calloc(order, order);
  convert_rowWise_to_gslMatrix(matrix, temp, order, order);
  
  gsl_linalg_cholesky_decomp(temp);
  gsl_linalg_cholesky_invert(temp);
  
  convert_gslMatrix_to_rowWise(temp, retVal, order, order);
  gsl_matrix_free(temp);
}

double MatrixHelper :: get(const double* m, int i, int j, int columnWidth) {
  return m[i*columnWidth + j]; // TODO: Double check if this assumption is correct through the class
}
void MatrixHelper :: set(double* m, int i, int j, int columnWidth, double val) {
  m[i*columnWidth + j] = val;
}

void MatrixHelper :: outerProductOfVectorsToCreateMatrix(double* vector1, double* vector2, double* retMatrix, int numDimensionsOfVector) {
  int index = 0;
  for(int i = 0; i < numDimensionsOfVector; i++) {
    for(int j = 0; j < numDimensionsOfVector; j++) {
      retMatrix[index] = vector1[i]*vector2[j];
      index++;
    }
  }
}

void MatrixHelper :: convert_rowWise_to_gslMatrix(const double* src, gsl_matrix * dest, int numRows, int numColumns) {
  for (int i = 0; i < numRows; i++) { // Convert double* matrix into gsl_matrix
    for (int j = 0; j < numColumns; j++) {
      gsl_matrix_set(dest, i , j, get(src, i, j, numColumns)); 
    }
  }
}

void MatrixHelper :: convert_gslMatrix_to_rowWise(const gsl_matrix* src, double* dest, int numRows, int numColumns) {
  for (int i = 0; i < numRows; i++) { // Convert double* matrix into gsl_matrix
    for (int j = 0; j < numColumns; j++) {
      set(dest, i, j, numColumns, gsl_matrix_get(src, i , j));
    }
  }
}

bool MatrixHelper :: areAllElementsFinite(const double* m, int numRows, int numColumns) {
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < numColumns; j++) {
      if(gsl_finite(get(m, i, j, numColumns)) == 0) {
        return false;
      }
    }
  }
  return true;
}

bool MatrixHelper :: areAllElementsFinite(const gsl_matrix* m, int numRows, int numColumns) {
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < numColumns; j++) {
      if(gsl_finite(gsl_matrix_get(m, i , j)) == 0) {
        return false;
      }
    }
  }
  return true;
}

bool MatrixHelper :: isPositiveDefinite(const double* m, int numRows, int numColumns) {
  gsl_matrix *temp = gsl_matrix_calloc(numRows, numColumns);
  convert_rowWise_to_gslMatrix(m, temp, numRows, numColumns);
  
  gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
  int errCode = gsl_linalg_cholesky_decomp(temp);
  gsl_set_error_handler(old_handler);
  
  if(errCode == GSL_EDOM) {
    gsl_matrix_free(temp);
    return false;
  }
  else {
    gsl_matrix_free(temp);
    return true;
  }
}

bool MatrixHelper :: isPositiveDefinite(const gsl_matrix* m, int numRows, int numColumns) {
  gsl_matrix *temp = gsl_matrix_calloc(numRows, numColumns);
  copy(m, temp, numRows, numColumns);
  
  gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
  int errCode = gsl_linalg_cholesky_decomp(temp);
  gsl_set_error_handler(old_handler);
  
  if(errCode == GSL_EDOM) {
    gsl_matrix_free(temp);
    return false;
  }
  else {
    gsl_matrix_free(temp);
    return true;
  }
}

void MatrixHelper :: copy(const gsl_matrix* src, gsl_matrix* dest, int numRows, int numColumns) {
  for(int i = 0; i < numRows; i++) {
    for(int j = 0; j < numColumns; j++) {
      gsl_matrix_set(dest, i, j, gsl_matrix_get(src, i, j));
    }
  }
}

std::string MatrixHelper :: getString(const gsl_matrix* m, int numRows, int numColumns, std::string columnSep, std::string rowSep) {
  assert(numRows > 0 && numColumns > 0 && m != NULL);
  std::stringstream ss;
  for(int i = 0; i < numRows; i++) {
    ss << gsl_matrix_get(m, i, 0);
    for(int j = 1; j < numColumns; j++) {
      ss << columnSep << gsl_matrix_get(m, i, j);
    }
    ss << rowSep;
  }
  return ss.str();
}

std::string MatrixHelper :: getString(const double* m, int numRows, int numColumns, std::string columnSep, std::string rowSep) {
  assert(numRows > 0 && numColumns > 0 && m != NULL);
  std::stringstream ss;
  for(int i = 0; i < numRows; i++) {
    ss << get(m, i, 0, numColumns);
    for(int j = 1; j < numColumns; j++) {
      ss << columnSep << get(m, i, j, numColumns);
    }
    ss << rowSep;
  }
  return ss.str();
}

// -----------------------------------------------------------------------------------------------

void StatisticsHelper :: rwishart(double* retVal, const unsigned int dof, const double* scale, int numDimension) {
  MatrixHelper mHelper;
  gsl_matrix *work = gsl_matrix_calloc(numDimension, numDimension);
  gsl_matrix *result = gsl_matrix_calloc(numDimension, numDimension);
  
  assert(dof > numDimension - 1);
  
  assert(mHelper.areAllElementsFinite(scale, numDimension, numDimension));
  mHelper.convert_rowWise_to_gslMatrix(scale, result, numDimension, numDimension);
  
  //assert(mHelper.areAllElementsFinite(result, numDimension, numDimension));
  
  if(mHelper.isPositiveDefinite(result, numDimension, numDimension) == 0) {
    HANDLE_GSL_ERROR("The scale matrix passed to rwishart is not positive definite");
  }
  
  
  for(int k = 0; k < numDimension; k++){
    gsl_matrix_set(work, k, k, sqrt(gsl_ran_chisq(global_random_number_generator, (dof-k))));
    
    for(int l = 0; l < k; l++) {
	    gsl_matrix_set(work, k, l, gsl_ran_ugaussian(global_random_number_generator));
    }
  }
  
  
  if(mHelper.isPositiveDefinite(result, numDimension, numDimension) == 0) {
    //assert(myMatrixHelper.areAllElementsFinite(result, numDimension, numDimension));
    std::cout << "Tx scale matrix:" << mHelper.getString(result, numDimension, numDimension, " ", ",");
    HANDLE_GSL_ERROR("Strange, scale matrix after transformation in rwishart is not positive definite");
  }
  
  gsl_linalg_cholesky_decomp(result);
  gsl_blas_dtrmm(CblasLeft,CblasLower,CblasNoTrans,CblasNonUnit,1.0,result,work);
  gsl_blas_dsyrk(CblasUpper,CblasNoTrans,1.0,work,0.0,result);
  
  mHelper.convert_gslMatrix_to_rowWise(result, retVal, numDimension, numDimension);
  
  gsl_matrix_free(work);
  gsl_matrix_free(result);
}

void StatisticsHelper :: riwishart(double* retVal, const unsigned int dof, const double* scale, int numDimension) {
  // 1. First invert the scale matrix:
  MatrixHelper myMatrixHelper;
  
  assert(dof > numDimension - 1);
  assert(myMatrixHelper.areAllElementsFinite(scale, numDimension, numDimension));
  assert(myMatrixHelper.isPositiveDefinite(scale, numDimension, numDimension));
  
  double* temp = new double[numDimension * numDimension];
  
  // If A ~ W(Sigma, nu), then inv(A) = X ~ InverseWishart(inv(Sigma), nu)
  myMatrixHelper.inv_cholesky_decomposition(scale, retVal, numDimension); // retVal <- inverse of scale matrix
  // assert(myMatrixHelper.isPositiveDefinite(retVal, numDimension, numDimension)); // No need to check because inverse of positive definite is also positive definite 
  rwishart(temp, dof, retVal, numDimension); // temp <- wishart sample
  myMatrixHelper.inv_cholesky_decomposition(temp, retVal, numDimension); // retVal <- inverse of wishart sample
  
  delete [] temp;
  
  assert(myMatrixHelper.areAllElementsFinite(retVal, numDimension, numDimension));
  assert(myMatrixHelper.isPositiveDefinite(retVal, numDimension, numDimension));
}


void StatisticsHelper :: rvmf(double* retVal, double* mean, double concentration, int numDimension) {
  VectorHelper vHelper;
  assert(ARE_DOUBLE_EQUAL(vHelper.absoluteValue(mean, numDimension), 1)); // Mean has to be unit vector
  assert(concentration >= 0);
  
  // This method is based on rejection sampling given in Wood's 94 paper: "Simulation of the von Mises-Fisher distribution"
  // Asaad Ganeiber's thesis "Estimation and Simulation in Directional and Statistical Shape model" presents the Wood's rejection sampling in much simpler form on Pg 106.
  // Idea: sampled vmf vector = [ sqrt(1 - W^2) V; W ]
  // where V = unit vector on sphere and W follows the density: f(w) = normalizing constant * (1 - w^2)^((p-3)/2) exp(k * w)
  // k = concentration and p = number of dimensions.
  double b0 = (-2 * concentration + sqrt(4*pow(concentration, 2) + pow(numDimension - 1, 2) )) /(numDimension - 1);
  double x0 = (1 - b0) / (1 + b0);
  double c = concentration * x0 + (numDimension - 1)*LOG(1 - pow(x0, 2));
  
  double W = 0;
  unsigned start_time = clock();
  double display_warning_after_secs = 1;
  bool displayWarning = false;
  
  while(true) {
    double Z = rbeta((numDimension - 1) / 2, (numDimension - 1) / 2);
    double U = runif(0, 1);
    W = (1 - (1 + b0) * Z) / (1 + (1 - b0) * Z);
    if((concentration * W + (numDimension - 1) * LOG(1 - x0 * W) - c) >= LOG(U)) {
      break;
    }
    
    // ------------- so that we don't go into infinite loop if concentration is way too high or something else -----------
    // This might happen for very high concentration and/or high dimensions.
    /*
    double num_seconds_elapsed = (clock() - start_time) / (double)CLOCKS_PER_SEC;
    if(num_seconds_elapsed >= display_warning_after_secs && !displayWarning) {
      std::cout << "Warning: The rejection sampling for Von-Mises Fisher distribution is taking way too long (more than " << num_seconds_elapsed << " seconds) for concentration=" << concentration << " and mean=";
      vHelper.print(mean, numDimension);
      std::cout << "\n";
      displayWarning = true;
    }
    if(num_seconds_elapsed >= 30) {
      // After 30 second quit the program
      std::cout << "Error: The rejection sampling for Von-Mises Fisher distribution took way too long (more than " << num_seconds_elapsed << " seconds). So exiting the program now.\n";
      exit(-1);
    }
     */
    // ---------------------------------------------------------------------------------------------------------------------
  }
  rspherical_uniform(retVal, numDimension - 1);
  for (int i = 0; i < numDimension - 1; i++) {
    retVal[i] *= sqrt(1 - pow(W, 2));
  }
  retVal[numDimension - 1] = W;
}

void StatisticsHelper :: rspherical_uniform(double* retVal, int numDimension) {
  // See http://mathoverflow.net/questions/24688/efficiently-sampling-points-uniformly-from-the-surface-of-an-n-sphere
  // This works because the multivariate normal (X1, ... ,Xn) with covariance matrix the identity (that is, n independent unit normals) is rotationally symmetric around the origin.
  
  double sumOfSquares = 0;
  for(int i = 0; i < numDimension; i++) {
    retVal[i] = rnorm(0, 1);
    sumOfSquares += pow(retVal[i], 2);
  }
  
  sumOfSquares = sqrt(sumOfSquares);
  
  for(int i = 0; i < numDimension; i++) {
    retVal[i] = retVal[i] / sumOfSquares;
  }
}

// Returns LOG(a' + b') where a = LOG(a') and b = LOG(b')
double  VectorHelper :: log_add(double a, double b) {
  // Note, a > b
  double min = a < b ? a : b;
  double max = a > b ? a : b;
  
  double temp = 1 + exp(min - max);
  if(temp == 0) {
    //HANDLE_GSL_ERROR("Problem while adding logs")
    assert(false);
  }
  assert(temp > 0);
  return max + log(temp);
}

void VectorHelper :: add(double* vector1, double* vector2, int sizeOfVector) {
  for(int i = 0; i < sizeOfVector; i++) {
    vector1[i] += vector2[i];
    assert(IS_FINITE(vector1[i]));
  }
}

double VectorHelper :: dot(double* vector1, double* vector2, int sizeOfVector) {
  // As sum += vector1[i] * vector2[i];
  double sum = 0;
  
  for(int i = 0; i < sizeOfVector; i++) {
    sum += vector1[i]*vector2[i];
  }
  assert(IS_FINITE(sum));
  return sum;
}

double* VectorHelper :: createEmptyVector(int sizeOfVector, double defaultValue) {
  double* temp = new double[sizeOfVector];
  for(int i = 0; i < sizeOfVector; i++) {
    temp[i] = defaultValue;
  }
  return temp;
}

void VectorHelper :: assignDefaultValue(double* vec, int sizeOfVector, double defaultValue) {
  for(int i = 0; i < sizeOfVector; i++) {
    vec[i] = defaultValue;
  }
}

void VectorHelper :: multiplyByConstant(double* vec, double constant, int sizeOfVector) {
  for(int i = 0; i < sizeOfVector; i++) {
    vec[i] *= constant;
    assert(IS_FINITE(vec[i]));
  }
}

// Performs retVec = vec1 + constant*vec2
void VectorHelper :: multiplyByConstantAndThenAdd(double* retVec, double* vec1, double* vec2, double constant, int sizeOfVector) {
  for(int i = 0; i < sizeOfVector; i++) {
    retVec[i] = vec1[i] + vec2[i]*constant;
    assert(IS_FINITE(retVec[i]));
  }
}

void VectorHelper :: copy(double* vecTo, double* vecFrom, int sizeOfVector) {
  for(int i = 0; i < sizeOfVector; i++) {
    vecTo[i] = vecFrom[i];
  }
}

void VectorHelper :: createUnitVector(double* retUnitVector, double* vector1, int sizeOfVector) {
  if(retUnitVector != vector1)
    copy(retUnitVector, vector1, sizeOfVector);
  
  double absVal = absoluteValue(vector1, sizeOfVector);
  multiplyByConstant(retUnitVector, pow((double)absVal, -1), sizeOfVector);
}

double VectorHelper :: absoluteValue(double* vec, int sizeOfVector) {
  double retVal = 0;
  for(int i = 0; i < sizeOfVector; i++) {
    double sq = pow(vec[i], 2);
    if(! IS_FINITE(sq)) {
      std::cout << "Square(" << vec[i] << ") is infinite";
      assert(false);
    }
    retVal += sq;
  }
  return sqrt(retVal);
}

void VectorHelper :: print(double* vec, int sizeOfVector) {
  assert(sizeOfVector >= 1);
  std::cout << "[" << vec[0];
  
  for(int i = 1; i < sizeOfVector; i++) {
    std::cout << " " << vec[i];
  }
  std::cout << " ]";
}

std::string VectorHelper :: getPrintString(double* vec, int sizeOfVector) {
  std::stringstream ss;
  if(vec == NULL) {
    return "NULL";
  }
  
  assert(sizeOfVector >= 1);
  
  ss << "[" << vec[0];
  
  for(int i = 1; i < sizeOfVector; i++) {
    ss << " " << vec[i];
  }
  ss << " ]";
  return ss.str();
}

void VectorHelper :: printWithConstantMultiplier(double* vec, double constant, int sizeOfVector) {
  std::cout << "[";
  for(int i = 0; i < sizeOfVector; i++) {
    std::cout << " " << vec[i] * constant;
  }
  std::cout << " ]";
}

void VectorHelper :: convertToUnitVector(double* vec, int numDimension) {
  double absVal = absoluteValue(vec, numDimension);
  multiplyByConstant(vec, pow((double)absVal, -1), numDimension);
}

double VectorHelper :: angle(double* vector1, double* vector2, int sizeOfVector, bool in_degrees) {
  double u_bar = absoluteValue(vector1, sizeOfVector);
  double v_bar = absoluteValue(vector2, sizeOfVector);
  double uv = dot(vector1, vector2, sizeOfVector);
  double angle_in_radians = acos(uv / (u_bar * v_bar));
  if(in_degrees) {
    return angle_in_radians * 180.0 / 3.14159265;
  }
  else {
    return angle_in_radians;
  }
}

void VectorHelper :: convert_rowWise_to_gslVector(const double* src, gsl_vector * dest, int sizeOfVector) {
  for(int i = 0; i < sizeOfVector; i++) {
    gsl_vector_set(dest, i, src[i]);
  }
}

void VectorHelper :: convert_gslVector_to_rowWise(const gsl_vector* src, double* dest, int sizeOfVector) {
  for(int i = 0; i < sizeOfVector; i++) {
    dest[i] = gsl_vector_get(src, i);
  }
}

